[TOC]

Overview

\[z = f(x) + n, \quad n \sim \mathcal{N}(0, \Sigma), \quad z \sim \mathcal{N}(f(x), \Sigma)\] \[P(z \mid x) = \mathcal{N}(z; f(x), \Sigma) = \eta \exp \left(-\frac{1}{2}(z-f(x))^{T} {\Sigma}^{-1}(z-f(x))\right)\]

EKF

true state

\[\begin{aligned} x_k &= f(x_{k-1}, w_{k-1}) \\ z_k &= h(x_k, v_k) \end{aligned}\]

norminal state

\[\begin{aligned} \bar{x}_k &= f(\hat{x}_{k-1}, 0) \\ \bar{z}_k &= h(\bar{x}, 0) \end{aligned}\]

true state linearization

\[\begin{aligned} x_k &\approx \bar{x}_k + A(x_{k-1} - \hat{x}_{k-1}) + W w_{k-1} \\ z_k &\approx \bar{z}_k + H(x_k - \bar{x}_k) + V v_k \end{aligned}\]

prediction error & measurement residual

\[\begin{aligned} e_x &\equiv x_k - \bar{x}_k \approx A(x_{k-1} - \hat{x}_{k-1}) + W w_{k-1} \\ e_z &\equiv z_k - \bar{z}_k \approx H e_x + V v_k \end{aligned}\]

jacobian

\[A = \frac{\partial f}{\partial x}, \quad W = \frac{\partial f}{\partial w}\] \[H = \frac{\partial h}{\partial x}, \quad V = \frac{\partial h}{\partial v}\]

covariance

\[w \sim \mathcal{N}(0, Q), \quad v \sim \mathcal{N}(0, R)\] \[e_x \sim \mathcal{N}(0, P) \rightarrow x \sim \mathcal{N}(\bar{x}, P), \quad e_z \sim \mathcal{N}(0, S)\] \[P = \texttt{cov}(e_x) = E(e_x e_x^T), \quad S = \texttt{cov}(e_z) = E(e_z e_z^T)\]

Prediction

state prediction (w/o noise)

\[\hat{x}_k^- = f(\hat{x}_{k-1}, 0)\]

(error state) covariance

\[\text{cov}(x_k - \hat{x}_k^-) = P_k^- = A_k P_{k-1} A_k^T + W_k Q_{k-1} W_k^T\]

Update

Kalman gain

\[K_k = P_k^- H_k^T S^{-1}, \quad S = H_k P_k^- H_k^T + V_k R_k V_k^T\]

state update

\[\hat{x}_k = \hat{x}_k^- + K_k (z_k - h(\hat{x}_k^-, 0))\]

covariance update

\[\text{cov}(x_k - \hat{x}_k) = P_k = (I - K_k H_k) P_k^-\]

MSCKF

Prediction

state prediction (state prior)

\[PVQ\]

propagate error cov P

continuous-time to discret-time，离散时间 状态转移矩阵和噪声协方差矩阵 比较准确，例如

\[F = \exp(A \Delta t) \approx I + A \Delta t + \frac{1}{2} (A \Delta t)^2 + \frac{1}{6} (A \Delta t)^3\]

误差状态的概率分布

\[\delta x \sim \mathcal{N}(\hat{\delta x}, P) , \quad n \sim \mathcal{N}(0, Q)\]

误差协方差传播（整个系统过程）

\[\delta x_{i+1} = F \delta x_i + G n , \quad P = F P F^T + G Q G^T\]

Update (ESKF)

\[\delta x \sim \mathcal{N}(\hat{\delta x}, P)\] \[z = h(x) + v \approx h(x_0) + H \delta x + v, \quad v \sim \mathcal{N}(0, R)\]

predicted residual (innovation)

\[r = z - h(x_0) = H \delta x + v\]

then, the covariance of innovation

\[cov(r, r) = E(rr^T) = E(H \delta x \delta x^T H^T + vv^T) = HPH^T + R\]

update state and covariance

\[x = K r\] \[P = (I-KH)P\]

MAP (VINS-Mono)

Prediction

state prediction (state prior)

\[PVQ\]

pre-integration (propagate error cov P & state)

continuous-time to discret-time

\[F = \exp(A \Delta t) \approx I + A \Delta t\]

误差状态的概率分布

\[\delta x \sim \mathcal{N}(0, P) , \quad n \sim \mathcal{N}(0, Q)\]

误差状态（状态预积分）和协方差传播（图像k时刻初始，图像k～图像k+1）

\[\delta x_{i+1} = F \delta x_i + G n , \quad P = F P F^T + G Q G^T\]

Update (MAP)

Jacobian & information matrix in MAP

\[J = \frac{\partial r}{\partial \delta x}\]

IMU

协方差矩阵（信息矩阵的逆）

\[\text{cov}(\delta x_k) = P\]

Cam

协方差矩阵（信息矩阵的逆）

\[\text{cov}(r_k) = \Sigma_{\pi}\]

update state

\[x = x + \delta x\]

QA

Jacobi when Linear and Nonlinear

欧式空间的非线性方程

\[h(x) \approx h(x_0) + H \Delta x, \quad \left. H = \frac{\partial h(x)}{\partial x} \right|_{x = x_0}\]

当 $h(x)$ 线性时

\[h(x) = Hx\]

Jacobi w.r.t Error or True State

\[f(x_0 \oplus \Delta x) = F(\Delta x)\] \[f(x_0 \oplus \Delta x) \approx f(x_0) + \left. \frac{\partial f(x_0 \oplus \Delta x)}{\partial x} \right|_{x=x_0} \Delta x\] \[F(\Delta x) \approx F(0) + \left. \frac{\partial F(\Delta x)}{\partial \Delta x} \right|_{\Delta x = 0} \Delta x = f(x_0) + \left. \frac{\partial f(x_0 \oplus \Delta x)}{\partial \Delta x} \right|_{\Delta x = 0} \Delta x\]

当x在欧式空间时，上式等价。

• ref: https://zhuanlan.zhihu.com/p/75714471

Jacobi in EKF & MAP

优化变量 是 什么状态，对应的 雅克比 即是 对什么状态 求导

EKF

	w.r.t true state	w.r.t error state
measurement function	$h(x)$	$h(\Delta x)$
Jacobi	$\frac{\partial h(x)}{\partial x}$	$\frac{\partial h(x)}{\partial \Delta x}$
init state	$x = x_0$	${\Delta x}=0$
update	$x \oplus \Delta x, \Delta x = Kr$	$\Delta x = Kr$

MAP

	w.r.t true-state	w.r.t error-state
cost function	$f(x)$	$f(\Delta x)$
Jacobi	$\frac{\partial f(x)}{\partial x}$	$\frac{\partial f(x)}{\partial \Delta x}$
init state	$x = x_0$	${\Delta x}=0$
iteration update	$x \oplus \delta x$	$\Delta x \oplus \delta \Delta x$