FAST-LIO公式推导

原代码对新手似乎不是很友好，所以用Eigen重新实现了Fast-LIO的核心功能：Github链接 Gitee链接

本文中变量命名与Fast-LIO论文中不尽相同，比如为了便于阅读而忽略了很多上下标，如有误解之处，敬请指正。

---

0. 运算符定义

通过指数/对数映射可以实现李群和李代数之间的映射，定义$⊞$和$\boxminus$运算符，如下：
$$\begin{gathered} R⊞r=RExp(r) \\ {R}_1\boxminus R_2=Log(R_2^T{R}_1) \\ a⊞b=a+b \\ a\boxminus b=a-b \end{gathered}$$
其中$R,R_1,R_2\in SO(3)$，$a,b\in {\mathbb{R}}^n$。指数映射为：
$$Exp(r)=I+\frac{r}{||r||}sin(||r||)+\frac{r^2}{||r|{|}^2}(1-cos(||r||))$$
上式也就是罗德里格斯公式，对数映射是它的逆映射。对应的程序如下：

// 指数映射
static Eigen::Matrix3d Exp(const Eigen::Vector3d& r){
    Eigen::Matrix3d expr;
    double theta = r.norm();
    if(theta < 1e-7){
        expr = Eigen::Matrix3d::Identity();
    }
    else{
        Eigen::Matrix3d skew = get_skew_symmetric(r / theta);
        expr = Eigen::Matrix3d::Identity() + sin(theta) * skew + (1 - cos(theta)) * skew * skew;
    }
    return expr;
}
// 对数映射
static Eigen::Vector3d Log(const Eigen::Matrix3d& R){
    double theta = (R.trace() > 3 - 1e-6) ? 0 : acos((R.trace() - 1) / 2);
    Eigen::Vector3d r(R(2,1) - R(1,2), R(0,2) - R(2,0), R(1,0) - R(0,1));
    return fabs(theta) < 0.001 ? (0.5 * r) : (0.5 * theta / sin(theta) * r);
}   
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

1. 系统模型

因为fastlio论文看着有些晦涩，这里的推导主要参考高翔的博文简明ESKF推导

定义状态向量$x$：
x = [ p T v T R T b g T b a T g T ] T x=

^T x=[pT​vT​RT​bgT​​baT​​gT​]T
状态的连续时间微分方程为：
$$\begin{gathered} \dot{p}=v \\ \dot{v}=R(f^b-b_a-n_a)-g \\ \dot{R}=R({\omega }^b-b_g-n_g)\times \\ \dot{b_g}=n_{bg} \\ \dot{b_a}=n_{ba} \\ \dot{g}=0 \end{gathered}$$
其中$f^b$和${\omega }^b$分别为加速度计和陀螺仪测量值。

状态向量的估计值$\hat{x}$，表示为：
x ^ = [ p ^ T v ^ T R ^ T b ^ g T b ^ a T g ^ T ] T \hat x=

^T x^=[p^​T​v^T​R^T​b^gT​​b^aT​​g^​T​]T
它的微分方程与真实值的微分方程相同，不过要忽略噪声。接下来推导误差状态向量$\delta x$，定义误差状态变量如下：

$$\begin{gathered} p=\hat{p}+\delta p \\ v=\hat{v}+\delta v \\ R=R\delta R \\ {b}_g={\hat{b}}_g+\delta b_g \\ {b}_a={\hat{b}}_a+\delta b_a \\ g=\hat{g}+\delta g \end{gathered}$$
姿态部分的误差$\delta R$对应的李代数为$\delta \theta$，即$\delta R=Exp(\delta \theta )$。因此真实状态、估计状态、误差状态三者的关系可以表述为：
$$x=\hat{x}⊞\delta x$$
其中误差状态向量 δ x = [ δ p T δ v T δ θ T δ b g T δ b a T δ g T ] T \delta x=

^T δx=[δpT​δvT​δθT​δbgT​​δbaT​​δgT​]T

误差状态中的位置、零偏和重力项都可以很容易的得到微分方程：
$$\begin{gathered} \delta \dot{p}=\delta v \\ \delta \dot{b_g}=n_{bg} \\ \delta \dot{b_a}=n_{ba} \\ \delta \dot{g}=0 \end{gathered}$$
速度、姿态均与$\delta R$相关，以下进行单独求导。

姿态误差

姿态真实值的微分方程：
$$\dot{R}=R({\omega }^b-b_g-n_g)\times \text{\hspace{1em}(1)}$$
姿态估计值的微分方程：
$$\dot{\hat{R}}=\hat{R}({\omega }^b-{\hat{b}}_g)\times \text{\hspace{1em}(2)}$$
姿态真实值、估计值、误差值三者的关系
$$R=\hat{R}Exp(\delta \theta )\text{\hspace{1em}(3)}$$

对$(3)$式两侧分别求时间导数，得到：
R ˙ = R ^ ˙ E x p ( δ θ ) + R ^ E x p ( δ θ ) ˙ = R ^ ˙ E x p ( δ θ ) + R ^ E x p ( δ θ ) ( δ θ ˙ × ) (4)

\tag{4} R˙​=R^˙Exp(δθ)+R^Exp(δθ)˙​=R^˙Exp(δθ)+R^Exp(δθ)(δθ˙×)​(4)

将$(3)$式代入$(1)$，得到：
R ˙ = R ^ E x p ( δ θ ) ( ω b − b g − n g ) × (5)

\tag{5} R˙​=R^Exp(δθ)(ωb−bg​−ng​)×​(5)

联立$(2)(4)(5)$，得到：
$$\hat{R}({\omega }^b-{\hat{b}}_g)\times Exp(\delta \theta )+\hat{R}Exp(\delta \theta )(\dot{\delta \theta }\times )=\hat{R}Exp(\delta \theta )({\omega }^b-b_g-n_g)\times$$
消掉$\hat{R}$，再利用$SO(3)$的伴随性质$\varphi \times R=R(R^T\varphi )\times$，得到：
$$Exp(\delta \theta )(\dot{\delta \theta }\times )=Exp(\delta \theta )({\omega }^b-b_g-n_g)\times -Exp(\delta \theta )(Exp(-\delta \theta )({\omega }^b-{\hat{b}}_g))\times$$
消掉$Exp(\delta \theta )$以及$\times$符号，得到：
δ θ ˙ = ( ω b − b g − n g ) − E x p ( − δ θ ) ( ω b − b ^ g ) ≈ ( ω b − b g − n g ) − ( I − δ θ × ) ( ω b − b ^ g ) = ( ω b − ( b ^ g + δ b g ) − n g ) − ( I − δ θ × ) ( ω b − b ^ g ) = − ( ω b − b g ) × δ θ − δ b g − n g

δθ˙​=(ωb−bg​−ng​)−Exp(−δθ)(ωb−b^g​)≈(ωb−bg​−ng​)−(I−δθ×)(ωb−b^g​)=(ωb−(b^g​+δbg​)−ng​)−(I−δθ×)(ωb−b^g​)=−(ωb−bg​)×δθ−δbg​−ng​​

速度误差

速度真实值的微分方程：
$$\dot{v}=R(f^b-b_a-n_a)-g\text{\hspace{1em}(1)}$$
估计值的微分方程：
$$\dot{\hat{v}}=\hat{R}(f^b-{\hat{b}}_a)-\hat{g}\text{\hspace{1em}(2)}$$
真实值、估计值、误差值的关系：
$$v=\hat{v}+\delta v\text{\hspace{1em}(3)}$$
分别对$(3)$式两侧求导，得到：
$$\dot{v}=\dot{\hat{v}}+\dot{\delta v}\text{\hspace{1em}(4)}$$
联立$(1)(2)(4)$，得到
R ^ ( f b − b ^ a ) − g ^ + δ v ˙ = R ( f b − b a − n a ) − g = R ^ E x p ( δ θ ) ( f b − b a − n a ) − g ≈ R ^ ( I + δ θ × ) ( f b − b a − n a ) − g

R^(fb−b^a​)−g^​+δv˙​=R(fb−ba​−na​)−g=R^Exp(δθ)(fb−ba​−na​)−g≈R^(I+δθ×)(fb−ba​−na​)−g​
化简得到：
$$\dot{\delta v}=\hat{R}(-\delta b_a-n_a-(f^b-b_a+\delta b_a-n_a)\times \delta \theta )-\delta g$$
忽略二次小量，得到：
δ v ˙ = − R ^ δ b a − R ^ ( f b − b a ) × δ θ − δ g − R ^ n a = − R ^ δ b a − R ^ ( f b − b a ) × δ θ − δ g − n a

δv˙=−R^δba−R^(fb−ba)×δθ−δg−R^na=−R^δba−R^(fb−ba)×δθ−δg−na" role="presentation" style="position: relative;">δv˙=−R^δba−R^(fb−ba)×δθ−δg−R^na=−R^δba−R^(fb−ba)×δθ−δg−na$$\begin{array}{rl}\dot{\delta v}& =-\hat{R}\delta b_a-\hat{R}(f^b-b_a)\times \delta \theta -\delta g-\hat{R}{n}_a\\ & =-\hat{R}\delta b_a-\hat{R}(f^b-b_a)\times \delta \theta -\delta g-n_a\end{array}$$

δv˙​=−R^δba​−R^(fb−ba​)×δθ−δg−R^na​=−R^δba​−R^(fb−ba​)×δθ−δg−na​​

状态方程

综上，误差状态的连续时间微分方程如下：
$$\begin{gathered} \delta \dot{p}=\delta v \\ \dot{\delta v}=-\hat{R}\delta b_a-\hat{R}(f^b-b_a)\times \delta \theta -\delta g-n_a \\ \dot{\delta \theta }=-({\omega }^b-b_g)\times \delta \theta -\delta b_g-n_g \\ \delta \dot{b_g}=n_{bg} \\ \delta \dot{b_a}=n_{ba} \\ \delta \dot{g}=0 \end{gathered}$$
离散后的微分方程如下：：
$$\begin{gathered} \delta p_{k+1}=\delta p_k+\delta v△t \\ \delta v_{k+1}=v_k-\hat{R}△t\delta b_a-\hat{R}(f^b-b_a)△t\times \delta \theta -\delta g△t-n_v \\ {\delta \theta }_{k+1}=Exp(-({\omega }^b-b_g)△t)\delta {\theta }_k-\delta b_g△t-n_{\theta } \\ \delta {b_g}_{k+1}=\delta {b_g}_k+n_g \\ \delta {b_a}_{k+1}=\delta {b_a}_k+n_a \\ \delta g_{k+1}=g_k \end{gathered}$$
上式可以记为：
$$\delta x_{k+1}=F_x\delta x_k+F_{w}w$$
其中：
w = [ n v T n θ T n g T n a T ] T ∼ N ( 0 , Q ) F x = [ I I △ t 0 0 0 0 0 I − R ^ ( f b − b a ) △ t × 0 − R ^ △ t I △ t 0 0 E x p ( − ( ω b − b g ) △ t ) − I △ t 0 0 0 0 0 I 0 0 0 0 0 0 I 0 0 0 0 0 0 I ] F w = [ 0 0 0 0 − I △ t 0 0 0 0 − I △ t 0 0 0 0 I △ t 0 0 0 0 I △ t 0 0 0 0 ]

wFx​Fw​​=[nvT​​nθT​​ngT​​naT​​]T∼N(0,Q)= ​I00000​I△tI0000​0−R^(fb−ba​)△t×Exp(−(ωb−bg​)△t)000​00−I△tI00​0−R^△t00I0​0I△t000I​ ​= ​0−I△t0000​00−I△t000​000I△t00​0000I△t0​ ​​
所以误差状态卡尔曼滤波的预测部分可以表述为：
$$\begin{gathered} \delta x_{k+1}=F_x\delta x_k \\ {P}_{k+1}=F_x{P}_k{F}_x^T+F_{w}Q{F}_w^T \end{gathered}$$
$\delta x$在更新后都会被重置为0，因此可以不进行计算。

2. 观测模型

观测值

以平面特征点的观测方程为例，首先使用下式将lidar系下的平面特征点$p_l$转换到world系下，其中忽略了lidar和imu的外参以及运动补偿。
$$p_w=R{p}_l+t$$
然后从地图中找到5个与$p_w$对应的平面特征点，用这些点拟合平面$Ax+By+Cz+D=0$，也即$\frac{A}{D}x+\frac{B}{D}y+\frac{C}{D}z=-1$。求解得到平面的归一化法向量$u=[A,B,C{]}^T$和截距$D$。然后计算点到平面的距离$z=Ax+By+Cz+D$。

观测方程

如果使用真实的参数$x$，计算得到的点到平面的距离应该为0，也就是说:
h ( x ) = h ( x ^ ⊞ δ x ) = u T ( R p l + t − q ) = u T ( ( R ^ ⊞ δ R ) p l + t ^ + t ~ − q ) = 0

h(x)​=h(x^⊞δx)=uT(Rpl​+t−q)=uT((R^⊞δR)pl​+t^+t −q)=0​
但是真实的参数无法获得，使用估计值$\hat{x}$计算出的距离为：
h ( x ^ ) = u T ( R ^ p l + t ^ − q )

h(x^)=uT(R^pl+t^−q)" role="presentation" style="position: relative;">h(x^)=uT(R^pl+t^−q)$$\begin{array}{rl}h(\hat{x})& =u^T(\hat{R}{p}_l+\hat{t}-q)\end{array}$$

h(x^)​=uT(R^pl​+t^−q)​

两者的偏差由姿态误差$\delta R$、平移误差$\delta t$和观测噪声引起。在$\delta x=0$处进行一阶泰勒展开
$$h(x)=h(\hat{x}⊞\delta x)+v\approx h(\hat{x})+H\delta x+v$$
$H$是$h(x)$在$\delta x=0$处的雅克比矩阵，变换上式得到：
$$H\delta x=h(\hat{x}⊞\delta x)-h(\hat{x})$$
对等式两侧分别求偏导，得到
H = ∂ H δ x ∂ δ x = ∂ ( h ( x ^ ⊞ δ x ) − h ( x ^ ) ) ∂ δ x = l i m δ x → 0 u T ( R ^ E x p ( δ θ ) p l + t ^ + δ t ) − u T ( R ^ p l + t ^ ) δ x

H​=∂δx∂Hδx​=∂δx∂(h(x^⊞δx)−h(x^))​=limδx→0​δxuT(R^Exp(δθ)pl​+t^+δt)−uT(R^pl​+t^)​​
分别求偏导$\frac{\partial H\delta x}{\partial \delta \theta }$和$\frac{\partial H\delta x}{\partial \delta t}$：
∂ H δ x ∂ δ θ = l i m δ θ → 0 u T ( R ^ E x p ( δ θ ) p l + t ^ ) − u T ( R ^ p l + t ^ ) δ θ = l i m δ θ → 0 u T ( R ^ E x p ( δ θ ) − R ^ ) p l δ θ ≈ l i m δ θ → 0 u T ( R ^ ( I + δ θ × ) − R ^ ) p l δ θ = l i m δ θ → 0 u T R ^ δ θ × p l δ θ = l i m δ θ → 0 − u T R ^ p l × δ θ δ θ = − u T R ^ ( p l × ) ∂ H δ x ∂ δ t = u T

∂Hδx∂δθ=limδθ→0⁡uT(R^Exp(δθ)pl+t^)−uT(R^pl+t^)δθ=limδθ→0⁡uT(R^Exp(δθ)−R^)plδθ≈limδθ→0⁡uT(R^(I+δθ×)−R^)plδθ=limδθ→0⁡uTR^δθ×plδθ=limδθ→0⁡−uTR^pl×δθδθ=−uTR^(pl×)∂Hδx∂δt=uT" role="presentation" style="position: relative;">∂Hδx∂δθ∂Hδx∂δt=limδθ→0uT(R^Exp(δθ)pl+t^)−uT(R^pl+t^)δθ=limδθ→0uT(R^Exp(δθ)−R^)plδθ≈limδθ→0uT(R^(I+δθ×)−R^)plδθ=limδθ→0uTR^δθ×plδθ=limδθ→0−uTR^pl×δθδθ=−uTR^(pl×)=uT$$\begin{array}{rl}\frac{\mathrm{\partial }H\delta x}{\mathrm{\partial }\delta \theta }& =\underset{\delta \theta \to 0}{lim}\frac{u^T(\hat{R}Exp(\delta \theta )p_l+\hat{t})-u^T(\hat{R}{p}_l+\hat{t})}{\delta \theta }\\ & =\underset{\delta \theta \to 0}{lim}\frac{u^T(\hat{R}Exp(\delta \theta )-\hat{R})p_l}{\delta \theta }\\ & \approx \underset{\delta \theta \to 0}{lim}\frac{u^T(\hat{R}(I+\delta \theta \times )-\hat{R})p_l}{\delta \theta }\\ & =\underset{\delta \theta \to 0}{lim}\frac{u^T\hat{R}\delta \theta \times p_l}{\delta \theta }\\ & =\underset{\delta \theta \to 0}{lim}\frac{-u^T\hat{R}{p}_l\times \delta \theta }{\delta \theta }\\ & =-u^T\hat{R}(p_l\times )\\ \\ \frac{\mathrm{\partial }H\delta x}{\mathrm{\partial }\delta t}& =u^T\end{array}$$

∂δθ∂Hδx​∂δt∂Hδx​​=limδθ→0​δθuT(R^Exp(δθ)pl​+t^)−uT(R^pl​+t^)​=limδθ→0​δθuT(R^Exp(δθ)−R^)pl​​≈limδθ→0​δθuT(R^(I+δθ×)−R^)pl​​=limδθ→0​δθuTR^δθ×pl​​=limδθ→0​δθ−uTR^pl​×δθ​=−uTR^(pl​×)=uT​
因此观测方程的雅克比矩阵表示为：
h i = [ u i T 0 − u i T R ^ ( p l i × ) 0 0 0 ] H = [ h 1 h 2 ⋮ h M ] h_i=

[uiT0−uiTR^(pli×)000]" role="presentation" style="position: relative;">[uTi0−uTiR^(pli×)000]$$\left[\begin{array}{cccccc}{u}_i^T& 0& -u_i^T\hat{R}({p_l}_i\times )& 0& 0& 0\end{array}\right]$$

\\ H=

[h1h2⋮hM]" role="presentation" style="position: relative;">⎡⎣⎢⎢⎢⎢h1h2⋮hM⎤⎦⎥⎥⎥⎥$$\left[\begin{array}{c}{h}_1\\ h_2\\ ⋮\\ h_M\end{array}\right]$$

hi​=[uiT​​0​−uiT​R^(pl​i​×)​0​0​0​]H= ​h1​h2​⋮hM​​ ​

3. 迭代更新

迭代卡尔曼滤波中的状态更新过程可以看做优化问题。

观测残差$\delta z$的条件分布

上一节中，有
h ( x ) = h ( x ^ ) + H δ x + v = 0

h(x)​=h(x^)+Hδx+v=0​
其中$v\sim N(0,R)$，因此有
− v = h ( x ^ ) + H δ x ∼ N ( 0 , R )

−v=h(x^)+Hδx∼N(0,R)" role="presentation" style="position: relative;">−v=h(x^)+Hδx∼N(0,R)$$\begin{array}{rl}-v& =h(\hat{x})+H\delta x\\ & \sim N(0,R)\end{array}$$

−v​=h(x^)+Hδx∼N(0,R)​

以上是观测残差$\delta z$在先验$\delta x$下的条件分布

先验分布

真实的误差状态$\delta x$与第$k$次迭代得到的误差状态$\delta x_k$之间的关系为
$$\delta x=x\boxminus \hat{x}={\hat{x}}_k⊞\delta x_k\boxminus \hat{x}$$
其中${\hat{x}}_k$是第$k$次迭代得到的状态向量，在$\delta x_k=0$处进行一阶泰勒展开，得到：
$$\delta x\approx {\hat{x}}_k\boxminus \hat{x}+J_k\delta x_k$$
$J_k$是$\delta x$在$\delta x_k=0$处的雅克比矩阵。可以轻易地得到，除了姿态误差外，其余项均为单位矩阵，以下求解姿态误差的雅克比矩阵$\frac{\delta \theta }{\delta {\theta }_k}$。

令真实值为$R$，对应的李代数为$\varphi$；预测值为$\hat{R}$，对应的李代数为$\hat{\varphi }$；误差状态为$\delta R$，对应的李代数为$\delta \theta$；第$k$次迭代的预测值为${\hat{R}}_k$，对应的李代数为${\hat{\varphi }}_k$；误差为$\delta {\theta }_k$。满足：
δ θ = R ⊟ R ^ = R ^ k ⊞ δ θ k ⊟ R ^ = L o g ( R ^ T R ^ k E x p ( δ θ k ) ) = L o g ( E x p ( − ϕ ^ ) E x p ( ϕ ^ k ) E x p ( δ θ k ) )

δθ​=R⊟R^=R^k​⊞δθk​⊟R^=Log(R^TR^k​Exp(δθk​))=Log(Exp(−ϕ^​)Exp(ϕ^​k​)Exp(δθk​))​
由右乘BCH近似得到：
$$\delta \theta \approx A^{-1}({\hat{\varphi }}_k\boxminus \hat{\varphi })\delta {\theta }_k+{\hat{\varphi }}_k\boxminus \hat{\varphi }$$

右乘BCH公式：
$$\begin{gathered} Log(Exp({\varphi }_1)Exp({\varphi }_2))\approx A^{-1}({\varphi }_1){\varphi }_2+{\varphi }_1,{\varphi }_2为小量 \\ A(\varphi )=I+\frac{sin(||\varphi ||)}{||\varphi ||}\frac{(\varphi \times {)}^2}{||\varphi |{|}^2}-\frac{1-cos(||\varphi ||)}{||\varphi ||}\frac{\varphi \times }{||\varphi ||} \end{gathered}$$
注：Fast-LIO中给出的A阵是左乘BCH近似雅克比矩阵，右乘雅克比矩阵自变量取负数或者转置即可得到左乘雅克比矩阵（所以论文里的A阵都进行了转置）
$$\begin{gathered} A_l(\varphi )=A_r(-\varphi ) \\ 或者A_l(\varphi )=A_r^T(\varphi ) \end{gathered}$$
(14讲4.3.1)

因此$\frac{\delta \theta }{\delta {\theta }_k}=A^{-1}({\hat{\varphi }}_k\boxminus \hat{\varphi })$。令 { J k = d i a g { I , I , A − 1 ( ϕ ^ k ⊟ ϕ ^ ) , I , I , I } d = x ^ k ⊟ x ^

{Jk​=diag{I,I,A−1(ϕ^​k​⊟ϕ^​),I,I,I}d=x^k​⊟x^​，则：
δ x = d + J k δ x k ∼ N ( 0 , P )

δx=d+Jkδxk∼N(0,P)" role="presentation" style="position: relative;">δx=d+Jkδxk∼N(0,P)$$\begin{array}{rl}\delta x& =d+J_k\delta x_k\\ & \sim N(0,P)\end{array}$$

δx​=d+Jk​δxk​∼N(0,P)​

求解最大后验估计

已知条件概率密度函数$p(\delta z|\delta x)$和先验概率密度函数$p(\delta x)$，条件概率分布和先验分布 { z + H δ x k ∼ N ( 0 , R ) d + J k δ x k ∼ N ( 0 , P )

{z+Hδxk​∼N(0,R)d+Jk​δxk​∼N(0,P)​均服从高斯分布，后验概率密度函数为：
p ( δ x ∣ δ z ) = p ( δ z ∣ δ x ) p ( δ x ) p ( δ z )

p(δx|δz)=p(δz|δx)p(δx)p(δz)" role="presentation" style="position: relative;">p(δx|δz)=p(δz|δx)p(δx)p(δz)$$\begin{array}{rl}p(\delta x|\delta z)& =\frac{p(\delta z|\delta x)p(\delta x)}{p(\delta z)}\end{array}$$

p(δx∣δz)​=p(δz)p(δz∣δx)p(δx)​​
最大后验估计定义为：求解$\delta x_k$，使得$p(\delta x|\delta z)$最大，即
m a x δ x k   p ( δ x ∣ δ z ) ∝ m a x δ x k   p ( δ z ∣ δ x ) p ( δ x ) ∝ m a x δ x k   e x p { − 1 2 ( z k + H δ x k ) T R − 1 ( z k + H δ x k ) − 1 2 ( d + J k δ x k ) T P − 1 ( d + J k δ x k ) } ∝   m i n δ x k { 1 2 ( z k + H δ x k ) T R − 1 ( z k + H δ x k ) + 1 2 ( d + J k δ x k ) T P − 1 ( d + J k δ x k ) } ∝   m i n δ x k ∣ ∣ d + J k δ x k ∣ ∣ P − 1 2 + ∣ ∣ z k + H δ x k ∣ ∣ R − 1 2

maxδxk⁡ p(δx|δz)∝maxδxk⁡ p(δz|δx)p(δx)∝maxδxk⁡ exp{−12(zk+Hδxk)TR−1(zk+Hδxk)−12(d+Jkδxk)TP−1(d+Jkδxk)}∝ minδxk⁡{12(zk+Hδxk)TR−1(zk+Hδxk)+12(d+Jkδxk)TP−1(d+Jkδxk)}∝ minδxk⁡||d+Jkδxk||P−12+||zk+Hδxk||R−12" role="presentation" style="position: relative;">maxδxk p(δx|δz)∝maxδxk p(δz|δx)p(δx)∝maxδxk exp{−12(zk+Hδxk)TR−1(zk+Hδxk)−12(d+Jkδxk)TP−1(d+Jkδxk)}∝ minδxk{12(zk+Hδxk)TR−1(zk+Hδxk)+12(d+Jkδxk)TP−1(d+Jkδxk)}∝ minδxk||d+Jkδxk||2P−1+||zk+Hδxk||2R−1$$\begin{array}{rl}\underset{\delta x_k}{max}p(\delta x|\delta z)& \propto \underset{\delta x_k}{max}p(\delta z|\delta x)p(\delta x)\\ & \propto \underset{\delta x_k}{max}exp\{-\frac{1}{2}(z_k+H\delta x_k{)}^T{R}^{-1}(z_k+H\delta x_k)-\frac{1}{2}(d+J_k\delta x_k{)}^T{P}^{-1}(d+J_k\delta x_k)\}\\ & \propto \underset{\delta x_k}{min}\{\frac{1}{2}(z_k+H\delta x_k{)}^T{R}^{-1}(z_k+H\delta x_k)+\frac{1}{2}(d+J_k\delta x_k{)}^T{P}^{-1}(d+J_k\delta x_k)\}\\ & \propto \underset{\delta x_k}{min}||d+J_k\delta x_k|{|}_{P^{-1}}^2+||z_k+H\delta x_k|{|}_{R^{-1}}^2\end{array}$$

maxδxk​​ p(δx∣δz)​∝maxδxk​​ p(δz∣δx)p(δx)∝maxδxk​​ exp{−21​(zk​+Hδxk​)TR−1(zk​+Hδxk​)−21​(d+Jk​δxk​)TP−1(d+Jk​δxk​)}∝ minδxk​​{21​(zk​+Hδxk​)TR−1(zk​+Hδxk​)+21​(d+Jk​δxk​)TP−1(d+Jk​δxk​)}∝ minδxk​​∣∣d+Jk​δxk​∣∣P−12​+∣∣zk​+Hδxk​∣∣R−12​​
将目标函数$ϵ$表示如下：
ϵ = 1 2 ∣ ∣ d + J k δ x k ∣ ∣ P − 1 2 + 1 2 ∣ ∣ z k + H δ x k ∣ ∣ R − 1 2 = 1 2 ( d + J k δ x k ) T P − 1 ( d + J k δ x k ) + 1 2 ( z k + H δ x k ) T R − 1 ( z k + H δ x k )

ϵ=12||d+Jkδxk||P−12+12||zk+Hδxk||R−12=12(d+Jkδxk)TP−1(d+Jkδxk)+12(zk+Hδxk)TR−1(zk+Hδxk)" role="presentation" style="position: relative;">ϵ=12||d+Jkδxk||2P−1+12||zk+Hδxk||2R−1=12(d+Jkδxk)TP−1(d+Jkδxk)+12(zk+Hδxk)TR−1(zk+Hδxk)$$\begin{array}{rl}ϵ& =\frac{1}{2}||d+J_k\delta x_k|{|}_{P^{-1}}^2+\frac{1}{2}||z_k+H\delta x_k|{|}_{R^{-1}}^2\\ & =\frac{1}{2}(d+J_k\delta x_k{)}^T{P}^{-1}(d+J_k\delta x_k)+\frac{1}{2}(z_k+H\delta x_k{)}^T{R}^{-1}(z_k+H\delta x_k)\end{array}$$

ϵ​=21​∣∣d+Jk​δxk​∣∣P−12​+21​∣∣zk​+Hδxk​∣∣R−12​=21​(d+Jk​δxk​)TP−1(d+Jk​δxk​)+21​(zk​+Hδxk​)TR−1(zk​+Hδxk​)​

求$\delta x_k$的偏导，得到：
∂ ϵ ∂ δ x k = ( J T P − 1 J + H T R − 1 H ) δ x k + J T P − 1 d + H T R − 1 z

∂δxk​∂ϵ​​=(JTP−1J+HTR−1H)δxk​+JTP−1d+HTR−1z​
令$\frac{\partial ϵ}{\partial \delta x_k}=0$，得到：
δ x k = ( J T P − 1 J + H T R − 1 H ) − 1 ( − J T P − 1 d − H T R − 1 z ) (1)

δxk=(JTP−1J+HTR−1H)−1(−JTP−1d−HTR−1z)" role="presentation" style="position: relative;">δxk=(JTP−1J+HTR−1H)−1(−JTP−1d−HTR−1z)$$\begin{array}{r}\delta x_k=(J^T{P}^{-1}J+H^T{R}^{-1}H{)}^{-1}(-J^T{P}^{-1}d-H^T{R}^{-1}z)\end{array}$$

\tag{1} δxk​=(JTP−1J+HTR−1H)−1(−JTP−1d−HTR−1z)​(1)
令$Q=(J^T{P}^{-1}J+H^T{R}^{-1}H{)}^{-1}$，由矩阵求逆定理，即$(A^{-1}+B{D}^{-1}C{)}^{-1}=A-AB(D+CAB{)}^{-1}CA$，得到：
$$Q=(I-(J^T{P}^{-1}J{)}^{-1}{H}^T(H(J^T{P}^{-1}J{)}^{-1}{H}^T+R{)}^{-1}H)(J^T{P}^{-1}J{)}^{-1}$$
令$K=(J^T{P}^{-1}J{)}^{-1}{H}^T(H(J^T{P}^{-1}J{)}^{-1}{H}^T+R{)}^{-1}$，此即卡尔曼增益。$Q$可表示为：
$$Q=(I-KH)(J^T{P}^{-1}J{)}^{-1}\text{\hspace{1em}(2)}$$
令$U=(J^T{P}^{-1}J{)}^{-1}$，联立 { K = U H T ( H U H T + R ) − 1 Q = ( I − K H ) U

{K=UHT(HUHT+R)−1Q=(I−KH)U" role="presentation" style="position: relative;">{K=UHT(HUHT+R)−1Q=(I−KH)U$$\{\begin{array}{l}K=U{H}^T(HU{H}^T+R{)}^{-1}\\ Q=(I-KH)U\end{array}$$

{K=UHT(HUHT+R)−1Q=(I−KH)U​，得到：
$$Q{H}^T=KR\text{\hspace{1em}(3)}$$
公式(3)推导过程如下：
对于公式$K=U{H}^T(HU{H}^T+R{)}^{-1}$，两侧同时右乘$HU{H}^T+R$，得到：
$$KHU{H}^T+KR=U{H}^T$$两侧同时减掉$KHU{H}^T$，得到：
K R = U H T − K H U H T = ( I − K H ) U H T

KR=UHT−KHUHT=(I−KH)UHT" role="presentation" style="position: relative;">KR=UHT−KHUHT=(I−KH)UHT$$\begin{array}{rl}KR& =U{H}^T-KHU{H}^T\\ & =(I-KH)U{H}^T\end{array}$$

KR​=UHT−KHUHT=(I−KH)UHT​将$Q=(I-KH)U$带入上式，即可得到公式(3)

将 { Q = ( I − K H ) ( J T P − 1 J ) − 1 Q H T = K R

{Q=(I−KH)(JTP−1J)−1QHT=KR​带入$(1)$式，得到
δ x k = ( J T P − 1 J + H T R − 1 H ) − 1 ( − J T P − 1 d − H T R − 1 z ) = Q ( − J T P − 1 d − H T R − 1 z ) = ( I − K H ) ( J T P − 1 J ) − 1 ( − J T P − 1 d ) − K R R − 1 z = − K z − ( I − K H ) J − 1 d

δxk=(JTP−1J+HTR−1H)−1(−JTP−1d−HTR−1z)=Q(−JTP−1d−HTR−1z)=(I−KH)(JTP−1J)−1(−JTP−1d)−KRR−1z=−Kz−(I−KH)J−1d" role="presentation" style="position: relative;">δxk=(JTP−1J+HTR−1H)−1(−JTP−1d−HTR−1z)=Q(−JTP−1d−HTR−1z)=(I−KH)(JTP−1J)−1(−JTP−1d)−KRR−1z=−Kz−(I−KH)J−1d$$\begin{array}{rl}\delta x_k& =(J^T{P}^{-1}J+H^T{R}^{-1}H{)}^{-1}(-J^T{P}^{-1}d-H^T{R}^{-1}z)\\ & =Q(-J^T{P}^{-1}d-H^T{R}^{-1}z)\\ & =(I-KH)(J^T{P}^{-1}J{)}^{-1}(-J^T{P}^{-1}d)-KR{R}^{-1}z\\ & =-Kz-(I-KH)J^{-1}d\end{array}$$

δxk​​=(JTP−1J+HTR−1H)−1(−JTP−1d−HTR−1z)=Q(−JTP−1d−HTR−1z)=(I−KH)(JTP−1J)−1(−JTP−1d)−KRR−1z=−Kz−(I−KH)J−1d​

更新当前迭代次数的状态${\hat{x}}_{k+1}$：
$${\hat{x}}_{k+1}={\hat{x}}_k⊞\delta x_k$$
所有迭代完成后，更新状态：
$$\begin{gathered} \overline{x}={\hat{x}}_{k+1} \\ \overline{P}=(I-KH)P \end{gathered}$$

卡尔曼增益变形

Fast-LIO利用矩阵求逆定理推导了一种新的卡尔曼增益计算形式
$$K=(P^{-1}+H^T{R}^{-1}H{)}^{-1}{H}^T{R}^{-1}$$
该方法将矩阵求逆运算的维数限制为状态的维数，而不是观测点云的数量，减少求逆的计算耗时。以下是推导过程。

由矩阵求逆定理：
$$(HP{H}^T+R{)}^{-1}=R^{-1}-R^{-1}H(P^{-1}+H^T{R}^{-1}H{)}^{-1}{H}^T{R}^{-1}$$
将上式代入到原始卡尔曼增益计算公式，得到：

K = P H T ( H P H T + R ) − 1 ⏟ 矩阵求逆定理 = P H T ( R − 1 − R − 1 H ( P − 1 + H T R − 1 H ) − 1 H T R − 1 ) = ( P H T − P H T R − 1 H ⏟ P ( P − 1 + H T R − 1 H ) − I ) ( P − 1 + H T R − 1 H ) − 1 H T ) R − 1 = ( P H T − P H T + ( P − 1 + H T R − 1 H ) − 1 H T ) R − 1 = ( P − 1 + H T R − 1 H ) − 1 H T R − 1

K​=PHT矩阵求逆定理 (HPHT+R)−1​​=PHT(R−1−R−1H(P−1+HTR−1H)−1HTR−1)=(PHT−P(P−1+HTR−1H)−I) PHTR−1H​​(P−1+HTR−1H)−1HT)R−1=(PHT−PHT+(P−1+HTR−1H)−1HT)R−1=(P−1+HTR−1H)−1HTR−1​

参考

FAST-LIO: A Fast, Robust LiDAR-inertial Odometry Package by Tightly-Coupled Iterated Kalman Filter

LINS: A Lidar-Inertial State Estimator for Robust and Efficient Navigation

IEKF-based Visual-Inertial Odometry using Direct Photometric Feedback

How to compute H

FAST-LIO2简明公式推导