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

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

0. 运算符定义

通过指数/对数映射可以实现李群和李代数之间的映射，定义⊞\boxplus⊞和⊟\boxminus⊟运算符，如下：
R⊞r=RExp(r)R1⊟R2=Log(R2TR1)a⊞b=a+ba⊟b=a−bR\boxplus r=RExp(r)\\ R_1\boxminus R_2=Log(R_2^TR_1)\\ a\boxplus b=a+b\\ a\boxminus b=a-b R⊞r=RExp(r)R1⊟R2=Log(R2TR1)a⊞b=a+ba⊟b=a−b
其中R,R1,R2∈SO(3)R,R_1,R_2\in SO(3)R,R1,R2∈SO(3)，a,b∈Rna,b\in \R^na,b∈Rn。指数映射为：
Exp(r)=I+r∣∣r∣∣sin(∣∣r∣∣)+r2∣∣r∣∣2(1−cos(∣∣r∣∣))Exp(r)=I+\frac{r}{||r||}sin(||r||)+\frac{r^2}{||r||^2}(1-cos(||r||)) Exp(r)=I+∣∣r∣∣rsin(∣∣r∣∣)+∣∣r∣∣2r2(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. 系统模型

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

定义状态向量xxx：
x=[pTvTRTbgTbaTgT]Tx=\begin{bmatrix}p^T&v^T&R^T&b_g^T&b_a^T&g^T\end{bmatrix}^T x=[pTvTRTbgTbaTgT]T
状态的连续时间微分方程为：
p˙=vv˙=R(fb−ba−na)−gR˙=R(ωb−bg−ng)×bg˙=nbgba˙=nbag˙=0\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 p˙=vv˙=R(fb−ba−na)−gR˙=R(ωb−bg−ng)×bg˙=nbgba˙=nbag˙=0
其中fbf^bfb和ωb\omega^bωb分别为加速度计和陀螺仪测量值。

状态向量的估计值x^\hat xx^，表示为：
x^=[p^Tv^TR^Tb^gTb^aTg^T]T\hat x=\begin{bmatrix}\hat p^T&\hat v^T&\hat R^T&\hat b_g^T&\hat b_a^T&\hat g^T\end{bmatrix}^T x^=[p^Tv^TR^Tb^gTb^aTg^T]T
它的微分方程与真实值的微分方程相同，不过要忽略噪声。接下来推导误差状态向量δx\delta xδx，定义误差状态变量如下：

p=p^+δpv=v^+δvR=RδRbg=b^g+δbgba=b^a+δbag=g^+δgp=\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 p=p^+δpv=v^+δvR=RδRbg=b^g+δbgba=b^a+δbag=g^+δg
姿态部分的误差δR\delta RδR对应的李代数为δθ\delta\thetaδθ，即δR=Exp(δθ)\delta R=Exp(\delta\theta)δR=Exp(δθ)。因此真实状态、估计状态、误差状态三者的关系可以表述为：
x=x^⊞δxx=\hat x\boxplus\delta x x=x^⊞δx
其中误差状态向量δx=[δpTδvTδθTδbgTδbaTδgT]T\delta x=\begin{bmatrix}\delta p^T&\delta v^T&\delta \theta^T&\delta b_g^T&\delta b_a^T&\delta g^T\end{bmatrix}^Tδx=[δpTδvTδθTδbgTδbaTδgT]T

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

姿态误差

姿态真实值的微分方程：
R˙=R(ωb−bg−ng)×(1)\dot{R}=R(\omega^b-b_g-n_g)\times\tag{1} R˙=R(ωb−bg−ng)×(1)
姿态估计值的微分方程：
R^˙=R^(ωb−b^g)×(2)\dot{\hat{R}}=\hat R(\omega^b-\hat b_g)\times\tag{2} R^˙=R^(ωb−b^g)×(2)
姿态真实值、估计值、误差值三者的关系
R=R^Exp(δθ)(3)R=\hat RExp(\delta\theta)\tag{3} R=R^Exp(δθ)(3)

对(3)(3)(3)式两侧分别求时间导数，得到：
R˙=R^˙Exp(δθ)+R^Exp(δθ)˙=R^˙Exp(δθ)+R^Exp(δθ)(δθ˙×)(4)\begin{aligned} \dot R&=\dot{\hat{R}}Exp(\delta\theta)+\hat R\dot{Exp(\delta\theta)}\\ &=\dot{\hat{R}}Exp(\delta\theta)+\hat RExp(\delta\theta)(\dot{\delta\theta}\times) \end{aligned}\tag{4} R˙=R^˙Exp(δθ)+R^Exp(δθ)˙=R^˙Exp(δθ)+R^Exp(δθ)(δθ˙×)(4)

将(3)(3)(3)式代入(1)(1)(1)，得到：
R˙=R^Exp(δθ)(ωb−bg−ng)×(5)\begin{aligned} \dot{R}&=\hat RExp(\delta\theta)(\omega^b-b_g-n_g)\times \end{aligned}\tag{5} R˙=R^Exp(δθ)(ωb−bg−ng)×(5)

联立(2)(4)(5)(2)(4)(5)(2)(4)(5)，得到：
R^(ωb−b^g)×Exp(δθ)+R^Exp(δθ)(δθ˙×)=R^Exp(δθ)(ωb−bg−ng)×\hat R(\omega^b-\hat b_g)\times Exp(\delta\theta)+\hat RExp(\delta\theta)(\dot{\delta\theta}\times)=\hat RExp(\delta\theta)(\omega^b-b_g-n_g)\times R^(ωb−b^g)×Exp(δθ)+R^Exp(δθ)(δθ˙×)=R^Exp(δθ)(ωb−bg−ng)×
消掉R^\hat RR^，再利用SO(3)SO(3)SO(3)的伴随性质ϕ×R=R(RTϕ)×\phi\times R=R(R^T\phi)\timesϕ×R=R(RTϕ)×，得到：
Exp(δθ)(δθ˙×)=Exp(δθ)(ωb−bg−ng)×−Exp(δθ)(Exp(−δθ)(ωb−b^g))×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(δθ)(δθ˙×)=Exp(δθ)(ωb−bg−ng)×−Exp(δθ)(Exp(−δθ)(ωb−b^g))×
消掉Exp(δθ)Exp(\delta\theta)Exp(δθ)以及×\times×符号，得到：
δθ˙=(ω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\begin{aligned} \dot{\delta\theta}&=(\omega^b-b_g-n_g)-Exp(-\delta\theta)(\omega^b-\hat b_g)\\ &≈(\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{aligned} δθ˙=(ω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

速度误差

速度真实值的微分方程：
v˙=R(fb−ba−na)−g(1)\dot{v}=R(f^b-b_a-n_a)-g\tag{1} v˙=R(fb−ba−na)−g(1)
估计值的微分方程：
v^˙=R^(fb−b^a)−g^(2)\dot{\hat {v}}=\hat R(f^b-\hat b_a)-\hat g\tag{2} v^˙=R^(fb−b^a)−g^(2)
真实值、估计值、误差值的关系：
v=v^+δv(3)v=\hat v+\delta v\tag{3} v=v^+δv(3)
分别对(3)(3)(3)式两侧求导，得到：
v˙=v^˙+δv˙(4)\dot v=\dot{\hat v}+\dot{\delta v}\tag{4} v˙=v^˙+δv˙(4)
联立(1)(2)(4)(1)(2)(4)(1)(2)(4)，得到
R^(fb−b^a)−g^+δv˙=R(fb−ba−na)−g=R^Exp(δθ)(fb−ba−na)−g≈R^(I+δθ×)(fb−ba−na)−g\begin{aligned} \hat R(f^b-\hat b_a)-\hat g+\dot{\delta v}&=R(f^b-b_a-n_a)-g\\ &=\hat RExp(\delta\theta)(f^b-b_a-n_a)-g\\ &≈\hat R(I+\delta\theta\times)(f^b-b_a-n_a)-g \end{aligned} R^(fb−b^a)−g^+δv˙=R(fb−ba−na)−g=R^Exp(δθ)(fb−ba−na)−g≈R^(I+δθ×)(fb−ba−na)−g
化简得到：
δv˙=R^(−δba−na−(fb−ba+δ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^(−δba−na−(fb−ba+δba−na)×δθ)−δg
忽略二次小量，得到：
δv˙=−R^δba−R^(fb−ba)×δθ−δg−R^na=−R^δba−R^(fb−ba)×δθ−δg−na\begin{aligned} \dot{\delta v}&=-\hat R\delta b_a-\hat R(f^b-b_a)\times\delta\theta-\delta g-\hat Rn_a\\ &=-\hat R\delta b_a-\hat R(f^b-b_a)\times\delta\theta-\delta g-n_a \end{aligned} δv˙=−R^δba−R^(fb−ba)×δθ−δg−R^na=−R^δba−R^(fb−ba)×δθ−δg−na

状态方程

综上，误差状态的连续时间微分方程如下：
δp˙=δvδv˙=−R^δba−R^(fb−ba)×δθ−δg−naδθ˙=−(ωb−bg)×δθ−δbg−ngδbg˙=nbgδba˙=nbaδg˙=0\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 δp˙=δvδv˙=−R^δba−R^(fb−ba)×δθ−δg−naδθ˙=−(ωb−bg)×δθ−δbg−ngδbg˙=nbgδba˙=nbaδg˙=0
离散后的微分方程如下：：
δpk+1=δpk+δv△tδvk+1=vk−R^△tδba−R^(fb−ba)△t×δθ−δg△t−nvδθk+1=Exp(−(ωb−bg)△t)δθk−δbg△t−nθδbgk+1=δbgk+ngδbak+1=δbak+naδgk+1=gk\delta{p}_{k+1}=\delta p_k+\delta v\triangle t\\ {\delta v_{k+1}}=v_k-\hat R\triangle t\delta b_a-\hat R(f^b-b_a)\triangle t\times\delta\theta-\delta g\triangle t-n_v\\ {\delta\theta}_{k+1}=Exp(-(\omega^b-b_g)\triangle t)\delta\theta_k-\delta b_g\triangle 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 δpk+1=δpk+δv△tδvk+1=vk−R^△tδba−R^(fb−ba)△t×δθ−δg△t−nvδθk+1=Exp(−(ωb−bg)△t)δθk−δbg△t−nθδbgk+1=δbgk+ngδbak+1=δbak+naδgk+1=gk
上式可以记为：
δxk+1=Fxδxk+Fww\delta x_{k+1}=F_x\delta x_k+F_ww δxk+1=Fxδxk+Fww
其中：
w=[nvTnθTngTnaT]T∼N(0,Q)Fx=[II△t00000I−R^(fb−ba)×0−R^△tI△t00Exp(−(ωb−bg)△t)−I△t00000I000000I000000I]Fw=[0000−I△t0000−I△t0000I△t0000I△t0000]\begin{aligned} w&=\begin{bmatrix}n_v^T&n_\theta^T&n_g^T&n_a^T\end{bmatrix}^T\sim N(0,Q)\\ F_x&=\begin{bmatrix} I&I\triangle t&0&0&0&0\\ 0&I&-\hat R(f^b-b_a)\times&0&-\hat R\triangle t&I\triangle t\\ 0&0&Exp(-(\omega^b-b_g)\triangle t)&-I\triangle t&0&0\\ 0&0&0&I&0&0\\ 0&0&0&0&I&0\\ 0&0&0&0&0&I \end{bmatrix}\\ F_w&=\begin{bmatrix} 0&0&0&0\\ -I\triangle t&0&0&0\\ 0&-I\triangle t&0&0\\ 0&0&I\triangle t&0\\ 0&0&0&I\triangle t\\ 0&0&0&0 \end{bmatrix} \end{aligned} wFxFw=[nvTnθTngTnaT]T∼N(0,Q)=⎣⎢⎢⎢⎢⎢⎢⎡I00000I△tI00000−R^(fb−ba)×Exp(−(ωb−bg)△t)00000−I△tI000−R^△t00I00I△t000I⎦⎥⎥⎥⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎢⎢⎡0−I△t000000−I△t000000I△t000000I△t0⎦⎥⎥⎥⎥⎥⎥⎤
所以误差状态卡尔曼滤波的预测部分可以表述为：
δxk+1=FxδxkPk+1=FxPkFxT+FwQFwT\delta x_{k+1}=F_x\delta x_k\\ P_{k+1}=F_xP_kF_x^T+F_wQF_w^T δxk+1=FxδxkPk+1=FxPkFxT+FwQFwT
δx\delta xδx在更新后都会被重置为0，因此可以不进行计算。

2. 观测模型

观测值

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

观测方程

如果使用真实的参数xxx，计算得到的点到平面的距离应该为0，也就是说:
h(x)=h(x^⊞δx)=uT(Rpl+t−q)=uT((R^⊞δR)pl+t^+t~−q)=0\begin{aligned} h(x) &=h(\hat x\boxplus\delta x)\\ &=u^T(Rp_l+t-q)\\ &=u^T((\hat{R}\boxplus\delta{R})p_l+\hat{t}+\widetilde{t}-q)\\ &=0 \end{aligned} h(x)=h(x^⊞δx)=uT(Rpl+t−q)=uT((R^⊞δR)pl+t^+t−q)=0
但是真实的参数无法获得，使用估计值x^\hat xx^计算出的距离为：
h(x^)=uT(R^pl+t^−q)\begin{aligned} h(\hat x)&=u^T(\hat Rp_l+\hat t-q)\\ \end{aligned} h(x^)=uT(R^pl+t^−q)

两者的偏差由姿态误差δR\delta{R}δR、平移误差δt\delta{t}δt和观测噪声引起。在δx=0\delta x=0δx=0处进行一阶泰勒展开
h(x)=h(x^⊞δx)+v≈h(x^)+Hδx+vh(x)=h(\hat x\boxplus\delta x)+v≈h(\hat x)+H\delta x+v h(x)=h(x^⊞δx)+v≈h(x^)+Hδx+v
HHH是h(x)h(x)h(x)在δx=0\delta x=0δx=0处的雅克比矩阵，变换上式得到：
Hδx=h(x^⊞δx)−h(x^)H\delta x=h(\hat x\boxplus\delta x)-h(\hat x) Hδx=h(x^⊞δx)−h(x^)
对等式两侧分别求偏导，得到
H=∂Hδx∂δx=∂(h(x^⊞δx)−h(x^))∂δx=limδx→0uT(R^Exp(δθ)pl+t^+δt)−uT(R^pl+t^)δx\begin{aligned} H&=\frac{\partial H\delta x}{\partial\delta x}\\ &=\frac{\partial (h(\hat x\boxplus\delta x)-h(\hat x))}{\partial\delta x}\\ &=\mathop{lim}_{\delta x\to0} \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{aligned} H=∂δx∂Hδx=∂δx∂(h(x^⊞δx)−h(x^))=limδx→0δxuT(R^Exp(δθ)pl+t^+δt)−uT(R^pl+t^)
分别求偏导∂Hδx∂δθ\frac{\partial H\delta x}{\partial\delta \theta}∂δθ∂Hδx和∂Hδx∂δt\frac{\partial H\delta x}{\partial\delta t}∂δt∂Hδx：
∂Hδx∂δθ=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×)∂Hδx∂δt=uT\begin{aligned} \frac{\partial H\delta x}{\partial\delta \theta}&= \mathop{lim}_{\delta\theta\to0} \frac{u^T(\hat{R}Exp(\delta\theta)p_l+\hat{t})-u^T(\hat{R}p_l+\hat{t})}{\delta\theta}\\ &=\mathop{lim}_{\delta\theta\to0} \frac{u^T(\hat{R}Exp(\delta\theta)-\hat{R})p_l}{\delta\theta}\\ &\approx \mathop{lim}_{\delta\theta\to0} \frac{u^T(\hat{R}(I+\delta\theta\times)-\hat{R})p_l}{\delta\theta}\\ &=\mathop{lim}_{\delta\theta\to0} \frac{u^T\hat{R}\delta\theta\times p_l}{\delta\theta}\\ &=\mathop{lim}_{\delta\theta\to0} \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{aligned} ∂δθ∂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
因此观测方程的雅克比矩阵表示为：
hi=[uiT0−uiTR^(pli×)000]H=[h1h2⋮hM]h_i=\begin{bmatrix}u_i^T&0&-u_i^T\hat{R}({p_l}_i\times)&0&0&0\end{bmatrix}\\ H=\begin{bmatrix}h_1\\h_2\\\vdots\\h_M\end{bmatrix} hi=[uiT0−uiTR^(pli×)000]H=⎣⎢⎢⎢⎡h1h2⋮hM⎦⎥⎥⎥⎤

3. 迭代更新

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

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

上一节中，有
h(x)=h(x^)+Hδx+v=0\begin{aligned} h(x)&=h(\hat x)+H\delta x+v\\ &=0 \end{aligned} h(x)=h(x^)+Hδx+v=0
其中v∼N(0,R)v\sim N(0,R)v∼N(0,R)，因此有
−v=h(x^)+Hδx∼N(0,R)\begin{aligned} -v&=h(\hat x)+H\delta x\\ &\sim N(0,R) \end{aligned} −v=h(x^)+Hδx∼N(0,R)

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

先验分布

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

令真实值为RRR，对应的李代数为ϕ\phiϕ；预测值为R^\hat{R}R^，对应的李代数为ϕ^\hat\phiϕ^；误差状态为δR\delta RδR，对应的李代数为δθ\delta\thetaδθ；第kkk次迭代的预测值为R^k\hat R_kR^k，对应的李代数为ϕ^k\hat\phi_kϕ^k；误差为δθk\delta\theta_kδθk。满足：
δθ=R⊟R^=R^k⊞δθk⊟R^=Log(R^TR^kExp(δθk))=Log(Exp(−ϕ^)Exp(ϕ^k)Exp(δθk))\begin{aligned} \delta\theta&=R\boxminus \hat R\\ &=\hat R_k\boxplus\delta\theta_k\boxminus \hat R\\ &=Log(\hat R^T\hat R_kExp(\delta\theta_k))\\ &=Log(Exp(-\hat\phi)Exp(\hat \phi_k)Exp(\delta\theta_k))\\ \end{aligned} δθ=R⊟R^=R^k⊞δθk⊟R^=Log(R^TR^kExp(δθk))=Log(Exp(−ϕ^)Exp(ϕ^k)Exp(δθk))
由右乘BCH近似得到：
δθ≈A−1(ϕ^k⊟ϕ^)δθk+ϕ^k⊟ϕ^\delta\theta≈A^{-1}(\hat\phi_k\boxminus\hat\phi)\delta\theta_k+\hat\phi_k\boxminus\hat\phi δθ≈A−1(ϕ^k⊟ϕ^)δθk+ϕ^k⊟ϕ^

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

因此δθδθk=A−1(ϕ^k⊟ϕ^)\frac{\delta \theta}{\delta\theta_k}=A^{-1}(\hat\phi_k\boxminus\hat\phi)δθkδθ=A−1(ϕ^k⊟ϕ^)。令{Jk=diag{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^，则：
δx=d+Jkδxk∼N(0,P)\begin{aligned} \delta x&=d+J_k\delta x_k\\ &\sim N(0,P) \end{aligned} δx=d+Jkδxk∼N(0,P)

求解最大后验估计

已知条件概率密度函数p(δz∣δx)p(\delta z|\delta x)p(δz∣δx)和先验概率密度函数p(δx)p(\delta x)p(δx)，条件概率分布和先验分布{z+Hδxk∼N(0,R)d+Jkδxk∼N(0,P)\begin{cases}z+H\delta x_k\sim N(0,R)\\d+J_k\delta x_k\sim N(0,P)\end{cases}{z+Hδxk∼N(0,R)d+Jkδxk∼N(0,P)均服从高斯分布，后验概率密度函数为：
p(δx∣δz)=p(δz∣δx)p(δx)p(δz)\begin{aligned} p(\delta x|\delta z)&=\frac{p(\delta z|\delta x)p(\delta x)}{p(\delta z)} \end{aligned} p(δx∣δz)=p(δz)p(δz∣δx)p(δx)
最大后验估计定义为：求解δxk\delta x_kδxk，使得p(δx∣δz)p(\delta x|\delta z)p(δx∣δz)最大，即
maxδxkp(δx∣δz)∝maxδxkp(δz∣δx)p(δx)∝maxδxkexp{−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\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
将目标函数ϵ\epsilonϵ表示如下：
ϵ=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)\begin{aligned} \epsilon&=\frac{1}{2}||d+J_k\delta x_k||^2_{P^{-1}}+\frac{1}{2}||z_k+H\delta x_k||^2_{R^{-1}}\\ &=\frac{1}{2}(d+J_k\delta x_k)^TP^{-1}(d+J_k\delta x_k)+\frac{1}{2}(z_k+H\delta x_k)^TR^{-1}(z_k+H\delta x_k)\\ \end{aligned} ϵ=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)

求δxk\delta x_kδxk的偏导，得到：
∂ϵ∂δxk=(JTP−1J+HTR−1H)δxk+JTP−1d+HTR−1z\begin{aligned} \frac{\partial\epsilon}{\partial\delta x_k}&=(J^TP^{-1}J+H^TR^{-1}H)\delta x_k+J^TP^{-1}d+H^TR^{-1}z\\ \end{aligned} ∂δxk∂ϵ=(JTP−1J+HTR−1H)δxk+JTP−1d+HTR−1z
令∂ϵ∂δxk=0\frac{\partial\epsilon}{\partial\delta x_k}=0∂δxk∂ϵ=0，得到：
δxk=(JTP−1J+HTR−1H)−1(−JTP−1d−HTR−1z)(1)\begin{aligned} \delta x_k=(J^TP^{-1}J+H^TR^{-1}H)^{-1}(-J^TP^{-1}d-H^TR^{-1}z) \end{aligned}\tag{1} δxk=(JTP−1J+HTR−1H)−1(−JTP−1d−HTR−1z)(1)
令Q=(JTP−1J+HTR−1H)−1Q=(J^TP^{-1}J+H^TR^{-1}H)^{-1}Q=(JTP−1J+HTR−1H)−1，由矩阵求逆定理，即(A−1+BD−1C)−1=A−AB(D+CAB)−1CA(A^{-1}+BD^{-1}C)^{-1}=A-AB(D+CAB)^{-1}CA(A−1+BD−1C)−1=A−AB(D+CAB)−1CA，得到：
Q=(I−(JTP−1J)−1HT(H(JTP−1J)−1HT+R)−1H)(JTP−1J)−1Q=(I-(J^TP^{-1}J)^{-1}H^T(H(J^TP^{-1}J)^{-1}H^T+R)^{-1}H)(J^TP^{-1}J)^{-1} Q=(I−(JTP−1J)−1HT(H(JTP−1J)−1HT+R)−1H)(JTP−1J)−1
令K=(JTP−1J)−1HT(H(JTP−1J)−1HT+R)−1K=(J^TP^{-1}J)^{-1}H^T(H(J^TP^{-1}J)^{-1}H^T+R)^{-1}K=(JTP−1J)−1HT(H(JTP−1J)−1HT+R)−1，此即卡尔曼增益。QQQ可表示为：
Q=(I−KH)(JTP−1J)−1(2)Q=(I-KH)(J^TP^{-1}J)^{-1}\tag{2} Q=(I−KH)(JTP−1J)−1(2)
令U=(JTP−1J)−1U=(J^TP^{-1}J)^{-1}U=(JTP−1J)−1，联立{K=UHT(HUHT+R)−1Q=(I−KH)U\begin{cases} K=UH^T(HUH^T+R)^{-1}\\ Q=(I-KH)U \end{cases}{K=UHT(HUHT+R)−1Q=(I−KH)U，得到：
Q=KRH−T(3)Q=KRH^{-T}\tag{3} Q=KRH−T(3)
将{Q=(I−KH)(JTP−1J)−1Q=KRH−T\begin{cases}Q=(I-KH)(J^TP^{-1}J)^{-1}\\Q=KRH^{-T}\end{cases}{Q=(I−KH)(JTP−1J)−1Q=KRH−T带入(1)(1)(1)式，得到
δxk=(JTP−1J+HTR−1H)−1(−JTP−1d−HTR−1z)=Q(−JTP−1d−HTR−1z)=(I−KH)(JTP−1J)−1(−JTP−1d)+KRH−T(−HTR−1z)=−Kz−(I−KH)J−1d\begin{aligned} \delta x_k&=(J^TP^{-1}J+H^TR^{-1}H)^{-1}(-J^TP^{-1}d-H^TR^{-1}z)\\ &=Q(-J^TP^{-1}d-H^TR^{-1}z)\\ &=(I-KH)(J^TP^{-1}J)^{-1}(-J^TP^{-1}d)+KRH^{-T}(-H^TR^{-1}z)\\ &=-Kz-(I-KH)J^{-1}d \end{aligned} δxk=(JTP−1J+HTR−1H)−1(−JTP−1d−HTR−1z)=Q(−JTP−1d−HTR−1z)=(I−KH)(JTP−1J)−1(−JTP−1d)+KRH−T(−HTR−1z)=−Kz−(I−KH)J−1d

更新当前迭代次数的状态x^k+1\hat x_{k+1}x^k+1：
x^k+1=x^k⊞δxk\hat x_{k+1}=\hat x_k\boxplus\delta x_{k} x^k+1=x^k⊞δxk
所有迭代完成后，更新状态：
x‾=x^k+1P‾=(I−KH)P\overline x=\hat x_{k+1}\\ \overline P=(I-KH)P x=x^k+1P=(I−KH)P

卡尔曼增益变形

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

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

K=PHT(HPHT+R)−1⏟矩阵求逆定理=PHT(R−1−R−1H(P−1+HTR−1H)−1HTR−1)=(PHT−PHTR−1H⏟P(P−1+HTR−1H)−I)(P−1+HTR−1H)−1HT)R−1=(PHT−PHT+(P−1+HTR−1H)−1HT)R−1=(P−1+HTR−1H)−1HTR−1\begin{aligned} K&=PH^T \underbrace{(HPH^T+R)^{-1}}_{矩阵求逆定理}\\ &=PH^T(R^{-1}-R^{-1}H(P^{-1}+H^TR^{-1}H)^{-1}H^TR^{-1})\\ &=(PH^T-\underbrace{PH^TR^{-1}H}_{P(P^{-1}+H^TR^{-1}H)-I)}(P^{-1}+H^TR^{-1}H)^{-1}H^T)R^{-1} \\ &=(PH^T-PH^T+(P^{-1}+H^TR^{-1}H)^{-1}H^T)R^{-1}\\ &=(P^{-1}+H^TR^{-1}H)^{-1}H^TR^{-1} \end{aligned} 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简明公式推导

FAST-LIO公式推导相关推荐

fast lio测试
fast lio2代码编译及运行编译 fast lio2仓库地址一. 安装依赖 ubuntu18+ros环境下,pcl和eigen可默认使用ros自带版本,所以只需安装 livox驱动包 livo ...
国内外LiDAR SLAM实验室总结
知乎上有一个关于视觉的SLAM实验室以及20年前的视觉SLAM开源算法的汇总SLAM 领域国内外优秀实验室汇总,非常详细,但是偏视觉一点,本文主要关注LiDAR SLAM. 这篇文章一开始是非常简短的 ...
迭代扩展卡尔曼滤波学习
论文名字<The iterated Kalman filter update as a Gauss-Newton method> 引言: 计算资源大,IKF比EKF效果更好.IKF的更新方 ...
四足机器人 | GEM(elevation map) + Fast_Lio(odometry) 环境部署记录
由于项目和科研需要,经常需要在不同的平台上(例如我的台式机和项目pc)部署 GEM(elevation map) + Fast_lio(odomtry) 的环境,因此记录下安装过程和过程中每次都会出现 ...
快速开平方根倒数算法(Fast inverse square root)的一点探究
文章目录一.写在前面 1. 提示 2. 背景与前情二.正文 1. 需求分析 2. 必备工具之IEEE-754浮点数表示方法 3. 同一储存单元32bits的两种不同意义 4. 公式推导 4. 本文 ...
立体匹配：经典算法Fast Bilateral Solver
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达作者丨HawkWang 来源丨计算摄影学点击进入->3D视觉工坊学习交流群一. 前言你好, ...
经典文献阅读之--LIO-PPF(增量平面预拟合LIO)
0. 简介自从ikd-tree出来后,现在越来越多的工作瞄准了增量式这种方法,比如说激光惯导里程计(LIDAR-Inertial Odometry,LIO)的高精度跟踪通常涉及最小化点到平面距离的k ...
我所理解的卡尔曼滤波——公式推导与应用
我所理解的卡尔曼滤波--公式推导与应用 1.什么是卡尔曼滤波 2.卡尔曼滤波的数学推导 2.1 状态方程和测量方程 2.2 卡尔曼滤波过程 3 卡尔曼滤波应用 1.什么是卡尔曼滤波先举个例子说一下什 ...
SIFT和SURF的替换算法——ORB (Oriented FAST and Rotated BRIEF 快速定向和旋转)
SIFT和SURF的替代算法--ORB (Oriented FAST and Rotated BRIEF 快速定向和旋转 1. 效果图 2. 源码参考 1. 用于关键点检测和描述的SIFT(Scal ...

FAST-LIO公式推导