FAST-LIO公式推导_fastlio公式

原代码对新手似乎不是很友好，所以用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={\left[\begin{array}{cccccc}{p}^T& v^T& R^T& b_g^T& b_a^T& g^T\end{array}\right]}^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}$，表示为：
$$\hat{x}={\left[\begin{array}{cccccc}{\hat{p}}^T& {\hat{v}}^T& {\hat{R}}^T& {\hat{b}}_g^T& {\hat{b}}_a^T& {\hat{g}}^T\end{array}\right]}^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$$
其中误差状态向量$\delta x={\left[\begin{array}{cccccc}\delta p^T& \delta v^T& \delta {\theta }^T& \delta b_g^T& \delta b_a^T& \delta g^T\end{array}\right]}^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)$式两侧分别求时间导数，得到：
$$\begin{array}{rl}\dot{R}& =\dot{\hat{R}}Exp(\delta \theta )+\hat{R}\dot{Exp(\delta \theta )}\\ & =\dot{\hat{R}}Exp(\delta \theta )+\hat{R}Exp(\delta \theta )(\dot{\delta \theta }\times )\end{array}\text{\hspace{1em}(4)}$$

将$(3)$式代入$(1)$，得到：
$$\begin{array}{rl}\dot{R}& =\hat{R}Exp(\delta \theta )({\omega }^b-b_g-n_g)\times \end{array}\text{\hspace{1em}(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$符号，得到：
$$\begin{array}{rl}\dot{\delta \theta }& =({\omega }^b-b_g-n_g)-Exp(-\delta \theta )({\omega }^b-{\hat{b}}_g)\\ & \approx ({\omega }^b-b_g-n_g)-(I-\delta \theta \times )({\omega }^b-{\hat{b}}_g)\\ & =({\omega }^b-({\hat{b}}_g+\delta b_g)-n_g)-(I-\delta \theta \times )({\omega }^b-{\hat{b}}_g)\\ & =-({\omega }^b-b_g)\times \delta \theta -\delta b_g-n_g\end{array}$$

速度误差

速度真实值的微分方程：
$$\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)$，得到
$$\begin{array}{rl}\hat{R}(f^b-{\hat{b}}_a)-\hat{g}+\dot{\delta v}& =R(f^b-b_a-n_a)-g\\ & =\hat{R}Exp(\delta \theta )(f^b-b_a-n_a)-g\\ & \approx \hat{R}(I+\delta \theta \times )(f^b-b_a-n_a)-g\end{array}$$
化简得到：
$$\dot{\delta v}=\hat{R}(-\delta b_a-n_a-(f^b-b_a+\delta b_a-n_a)\times \delta \theta )-\delta g$$
忽略二次小量，得到：
$$\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}$$

状态方程

综上，误差状态的连续时间微分方程如下：
$$\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$$
其中：
$$\begin{array}{rl}w& ={\left[\begin{array}{cccc}{n}_v^T& n_{\theta }^T& n_g^T& n_a^T\end{array}\right]}^T\sim N(0,Q)\\ F_x& =\left[\begin{array}{cccccc}I& I△t& 0& 0& 0& 0\\ 0& I& -\hat{R}(f^b-b_a)△t\times & 0& -\hat{R}△t& I△t\\ 0& 0& Exp(-({\omega }^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\end{array}\right]\\ F_w& =\left[\begin{array}{cccc}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\end{array}\right]\end{array}$$
所以误差状态卡尔曼滤波的预测部分可以表述为：
$$\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，也就是说:
$$\begin{array}{rl}h(x)& =h(\hat{x}⊞\delta x)\\ & =u^T(R{p}_l+t-q)\\ & =u^T((\hat{R}⊞\delta R)p_l+\hat{t}+\tilde{t}-q)\\ & =0\end{array}$$
但是真实的参数无法获得，使用估计值$\hat{x}$计算出的距离为：
$$\begin{array}{rl}h(\hat{x})& =u^T(\hat{R}{p}_l+\hat{t}-q)\end{array}$$

两者的偏差由姿态误差$\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})$$
对等式两侧分别求偏导，得到
$$\begin{array}{rl}H& =\frac{\partial H\delta x}{\partial \delta x}\\ & =\frac{\partial (h(\hat{x}⊞\delta x)-h(\hat{x}))}{\partial \delta x}\\ & ={\lim }_{\delta x\to 0}\frac{u^T(\hat{R}Exp(\delta \theta )p_l+\hat{t}+\delta t)-u^T(\hat{R}{p}_l+\hat{t})}{\delta x}\end{array}$$
分别求偏导$\frac{\partial H\delta x}{\partial \delta \theta }$和$\frac{\partial H\delta x}{\partial \delta t}$：
$$\begin{array}{rl}\frac{\partial H\delta x}{\partial \delta \theta }& ={\lim }_{\delta \theta \to 0}\frac{u^T(\hat{R}Exp(\delta \theta )p_l+\hat{t})-u^T(\hat{R}{p}_l+\hat{t})}{\delta \theta }\\ & ={\lim }_{\delta \theta \to 0}\frac{u^T(\hat{R}Exp(\delta \theta )-\hat{R})p_l}{\delta \theta }\\ & \approx {\lim }_{\delta \theta \to 0}\frac{u^T(\hat{R}(I+\delta \theta \times )-\hat{R})p_l}{\delta \theta }\\ & ={\lim }_{\delta \theta \to 0}\frac{u^T\hat{R}\delta \theta \times p_l}{\delta \theta }\\ & ={\lim }_{\delta \theta \to 0}\frac{-u^T\hat{R}{p}_l\times \delta \theta }{\delta \theta }\\ & =-u^T\hat{R}(p_l\times )\\ & \\ \frac{\partial H\delta x}{\partial \delta t}& =u^T\end{array}$$
因此观测方程的雅克比矩阵表示为：
$$\begin{gathered} h_i=\left[\begin{array}{cccccc}{u}_i^T& 0& -u_i^T\hat{R}({p_l}_i\times )& 0& 0& 0\end{array}\right] \\ H=\left[\begin{array}{c}{h}_1\\ h_2\\ ⋮\\ h_M\end{array}\right] \end{gathered}$$

3. 迭代更新

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

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

上一节中，有
$$\begin{array}{rl}h(x)& =h(\hat{x})+H\delta x+v\\ & =0\end{array}$$
其中$v\sim N(0,R)$，因此有
$$\begin{array}{rl}-v& =h(\hat{x})+H\delta x\\ & \sim N(0,R)\end{array}$$

以上是观测残差$\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$。满足：
$$\begin{array}{rl}\delta \theta & =R\boxminus \hat{R}\\ & ={\hat{R}}_k⊞\delta {\theta }_k\boxminus \hat{R}\\ & =Log({\hat{R}}^T{\hat{R}}_{k}Exp(\delta {\theta }_k))\\ & =Log(Exp(-\hat{\varphi })Exp({\hat{\varphi }}_k)Exp(\delta {\theta }_k))\end{array}$$
由右乘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 ^ $$\begin{cases}J_k=diag\{I,I,A^{-1}(\hat\phi_k\boxminus\hat\phi),I,I,I\}\\d=\hat x_k\boxminus\hat x\end{cases}$$ {Jk​=diag{I,I,A−1(ϕ^​k​⊟ϕ^​),I,I,I}d=x^k​⊟x^​，则：
$$\begin{array}{rl}\delta x& =d+J_k\delta x_k\\ & \sim N(0,P)\end{array}$$

求解最大后验估计

已知条件概率密度函数$p(\delta z|\delta x)$和先验概率密度函数$p(\delta x)$，条件概率分布和先验分布$\{\begin{array}{l}z+H\delta x_k\sim N(0,R)\\ d+J_k\delta x_k\sim N(0,P)\end{array}$均服从高斯分布，后验概率密度函数为：
$$\begin{array}{rl}p(\delta x|\delta z)& =\frac{p(\delta z|\delta x)p(\delta x)}{p(\delta z)}\end{array}$$
最大后验估计定义为：求解$\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 $$\begin{aligned} \mathop{max}_{\delta x_k}\ p(\delta x|\delta z)&\propto \mathop{max}_{\delta x_k}\ p(\delta z|\delta x)p(\delta x)\\ &\propto \mathop{max}_{\delta x_k}\ exp\{-\frac{1}{2}(z_k+H\delta x_k)^TR^{-1}(z_k+H\delta x_k)-\frac{1}{2}(d+J_k\delta x_k)^TP^{-1}(d+J_k\delta x_k)\}\\ &\propto\ \mathop{min}_{\delta x_k}\{\frac{1}{2}(z_k+H\delta x_k)^TR^{-1}(z_k+H\delta x_k)+\frac{1}{2}(d+J_k\delta x_k)^TP^{-1}(d+J_k\delta x_k)\}\\ &\propto\ \mathop{min}_{\delta x_k} ||d+J_k\delta x_k||^2_{P^{-1}}+||z_k+H\delta x_k||^2_{R^{-1}} \end{aligned}$$ 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​​
将目标函数$ϵ$表示如下：
$$\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}$$

求$\delta x_k$的偏导，得到：
$$\begin{array}{rl}\frac{\partial ϵ}{\partial \delta x_k}& =(J^T{P}^{-1}J+H^T{R}^{-1}H)\delta x_k+J^T{P}^{-1}d+H^T{R}^{-1}z\end{array}$$
令$\frac{\partial ϵ}{\partial \delta x_k}=0$，得到：
$$\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}\text{\hspace{1em}(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}$，联立$\{\begin{array}{l}K=U{H}^T(HU{H}^T+R{)}^{-1}\\ Q=(I-KH)U\end{array}$，得到：
$$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$，得到：
$$\begin{array}{rl}KR& =U{H}^T-KHU{H}^T\\ & =(I-KH)U{H}^T\end{array}$$将$Q=(I-KH)U$带入上式，即可得到公式(3)

将$\{\begin{array}{l}Q=(I-KH)(J^T{P}^{-1}J{)}^{-1}\\ Q{H}^T=KR\end{array}$带入$(1)$式，得到
$$\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}$$

更新当前迭代次数的状态${\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}$$
将上式代入到原始卡尔曼增益计算公式，得到：

$$\begin{array}{rl}K& =P{H}^T\underset{矩阵求逆定理}{\underbrace{(HP{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-\underset{P(P^{-1}+H^T{R}^{-1}H)-I)}{\underbrace{P{H}^T{R}^{-1}H}}(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}\end{array}$$

参考

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简明公式推导