EKF v.s. MSCKF v.s. MAP (VINS-Mono)

[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$