FAST-LIO公式推导

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

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


0. 运算符定义

通过指数/对数映射可以实现李群和李代数之间的映射,定义 ⊞ \boxplus ⊟ \boxminus 运算符,如下:
R ⊞ r = R E x p ( r ) R 1 ⊟ R 2 = L o g ( R 2 T R 1 ) a ⊞ b = a + b a ⊟ b = a − b R\boxplus r=RExp(r)\\ R_1\boxminus R_2=Log(R_2^TR_1)\\ a\boxplus b=a+b\\ a\boxminus b=a-b Rr=RExp(r)R1R2=Log(R2TR1)ab=a+bab=ab
其中 R , R 1 , R 2 ∈ S O ( 3 ) R,R_1,R_2\in SO(3) R,R1,R2SO(3) a , b ∈ R n a,b\in \R^n a,bRn。指数映射为:
E x p ( r ) = I + r ∣ ∣ r ∣ ∣ s i n ( ∣ ∣ r ∣ ∣ ) + r 2 ∣ ∣ r ∣ ∣ 2 ( 1 − c o s ( ∣ ∣ r ∣ ∣ ) ) Exp(r)=I+\frac{r}{||r||}sin(||r||)+\frac{r^2}{||r||^2}(1-cos(||r||)) Exp(r)=I+rrsin(r)+r2r2(1cos(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推导

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

状态向量的估计值 x ^ \hat x x^,表示为:
x ^ = [ p ^ T v ^ T R ^ T b ^ g T b ^ a T g ^ 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 ^ + δ p v = v ^ + δ v R = R δ R b g = b ^ g + δ b g b a = b ^ a + δ b a g = g ^ + δ g 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 p=p^+δpv=v^+δvR=RδRbg=b^g+δbgba=b^a+δbag=g^+δg
姿态部分的误差 δ R \delta R δR对应的李代数为 δ θ \delta\theta δθ,即 δ R = E x p ( δ θ ) \delta R=Exp(\delta\theta) δR=Exp(δθ)。因此真实状态、估计状态、误差状态三者的关系可以表述为:
x = x ^ ⊞ δ x x=\hat x\boxplus\delta x x=x^δx
其中误差状态向量 δ x = [ δ p T δ v T δ θ T δ b g T δ b a T δ g T ] 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 δ b g ˙ = n b g δ b a ˙ = n b a δ 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 − b g − n g ) × (1) \dot{R}=R(\omega^b-b_g-n_g)\times\tag{1} R˙=R(ωbbgng)×(1)
姿态估计值的微分方程:
R ^ ˙ = R ^ ( ω b − b ^ g ) × (2) \dot{\hat{R}}=\hat R(\omega^b-\hat b_g)\times\tag{2} R^˙=R^(ωbb^g)×(2)
姿态真实值、估计值、误差值三者的关系
R = R ^ E x p ( δ θ ) (3) R=\hat RExp(\delta\theta)\tag{3} R=R^Exp(δθ)(3)

( 3 ) (3) (3)式两侧分别求时间导数,得到:
R ˙ = R ^ ˙ E x p ( δ θ ) + R ^ E x p ( δ θ ) ˙ = R ^ ˙ E x p ( δ θ ) + R ^ E x p ( δ θ ) ( δ θ ˙ × ) (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 ^ E x p ( δ θ ) ( ω b − b g − n g ) × (5) \begin{aligned} \dot{R}&=\hat RExp(\delta\theta)(\omega^b-b_g-n_g)\times \end{aligned}\tag{5} R˙=R^Exp(δθ)(ωbbgng)×(5)

联立 ( 2 ) ( 4 ) ( 5 ) (2)(4)(5) (2)(4)(5),得到:
R ^ ( ω b − b ^ g ) × E x p ( δ θ ) + R ^ E x p ( δ θ ) ( δ θ ˙ × ) = R ^ E x p ( δ θ ) ( ω b − b g − n g ) × \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^(ωbb^g)×Exp(δθ)+R^Exp(δθ)(δθ˙×)=R^Exp(δθ)(ωbbgng)×
消掉 R ^ \hat R R^,再利用 S O ( 3 ) SO(3) SO(3)的伴随性质 ϕ × R = R ( R T ϕ ) × \phi\times R=R(R^T\phi)\times ϕ×R=R(RTϕ)×,得到:
E x p ( δ θ ) ( δ θ ˙ × ) = E x p ( δ θ ) ( ω b − b g − n g ) × − E x p ( δ θ ) ( E x p ( − δ θ ) ( ω 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(δθ)(ωbbgng)×Exp(δθ)(Exp(δθ)(ωbb^g))×
消掉 E x p ( δ θ ) Exp(\delta\theta) Exp(δθ)以及 × \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 \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} δθ˙=(ωbbgng)Exp(δθ)(ωbb^g)(ωbbgng)(Iδθ×)(ωbb^g)=(ωb(b^g+δbg)ng)(Iδθ×)(ωbb^g)=(ωbbg)×δθδbgng

速度误差

速度真实值的微分方程:
v ˙ = R ( f b − b a − n a ) − g (1) \dot{v}=R(f^b-b_a-n_a)-g\tag{1} v˙=R(fbbana)g(1)
估计值的微分方程:
v ^ ˙ = R ^ ( f b − b ^ a ) − g ^ (2) \dot{\hat {v}}=\hat R(f^b-\hat b_a)-\hat g\tag{2} v^˙=R^(fbb^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 ^ ( 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 \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^(fbb^a)g^+δv˙=R(fbbana)g=R^Exp(δθ)(fbbana)gR^(I+δθ×)(fbbana)g
化简得到:
δ v ˙ = R ^ ( − δ b a − n a − ( f b − b a + δ b a − n a ) × δ θ ) − δ 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^(δbana(fbba+δbana)×δθ)δ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 \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^δbaR^(fbba)×δθδgR^na=R^δbaR^(fbba)×δθδgna

状态方程

综上,误差状态的连续时间微分方程如下:
δ p ˙ = δ v δ v ˙ = − R ^ δ b a − R ^ ( f b − b a ) × δ θ − δ g − n a δ θ ˙ = − ( ω b − b g ) × δ θ − δ b g − n g δ b g ˙ = n b g δ b a ˙ = n b a δ 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^δbaR^(fbba)×δθδgnaδθ˙=(ωbbg)×δθδbgngδbg˙=nbgδba˙=nbaδg˙=0
离散后的微分方程如下::
δ p k + 1 = δ p k + δ v △ t δ v k + 1 = v k − R ^ △ t δ b a − R ^ ( f b − b a ) △ t × δ θ − δ g △ t − n v δ θ k + 1 = E x p ( − ( ω b − b g ) △ t ) δ θ k − δ b g △ t − n θ δ b g k + 1 = δ b g k + n g δ b a k + 1 = δ b a k + n a δ g k + 1 = g k \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+δvtδvk+1=vkR^tδbaR^(fbba)t×δθδgtnvδθk+1=Exp((ωbbg)t)δθkδbgtnθδbgk+1=δbgk+ngδbak+1=δbak+naδgk+1=gk
上式可以记为:
δ x k + 1 = F x δ x k + F w w \delta x_{k+1}=F_x\delta x_k+F_ww δxk+1=Fxδxk+Fww
其中:
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 ) × 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 ] \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]TN(0,Q)=I00000ItI00000R^(fbba)×Exp((ωbbg)t)00000ItI000R^t00I00It000I=0It000000It000000It000000It0
所以误差状态卡尔曼滤波的预测部分可以表述为:
δ x k + 1 = F x δ x k P k + 1 = F x P k F x T + F w Q F w T \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系下的平面特征点 p l p_l pl转换到world系下,其中忽略了lidar和imu的外参以及运动补偿
p w = R p l + t p_w=Rp_l+t pw=Rpl+t
然后从地图中找到5个与 p w p_w pw对应的平面特征点,用这些点拟合平面 A x + B y + C z + D = 0 Ax+By+Cz+D=0 Ax+By+Cz+D=0,也即 A D x + B D y + C D z = − 1 \frac{A}{D}x+\frac{B}{D}y+\frac{C}{D}z=-1 DAx+DBy+DCz=1。求解得到平面的归一化法向量 u = [ A , B , C ] T u=[A,B,C]^T u=[A,B,C]T和截距 D D D。然后计算点到平面的距离 z = A x + B y + C z + D z=Ax+By+Cz+D z=Ax+By+Cz+D

观测方程

如果使用真实的参数 x x x,计算得到的点到平面的距离应该为0,也就是说:
h ( x ) = h ( x ^ ⊞ δ x ) = u T ( R p l + t − q ) = u T ( ( R ^ ⊞ δ R ) p l + 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+tq)=uT((R^δR)pl+t^+t q)=0
但是真实的参数无法获得,使用估计值 x ^ \hat x x^计算出的距离为:
h ( x ^ ) = u T ( R ^ p l + 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 + v h(x)=h(\hat x\boxplus\delta x)+v≈h(\hat x)+H\delta x+v h(x)=h(x^δx)+vh(x^)+Hδx+v
H H H 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 = l i m δ x → 0 u T ( R ^ E x p ( δ θ ) p l + t ^ + δ t ) − u T ( R ^ p l + 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=δxHδx=δx(h(x^δx)h(x^))=limδx0δ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} δtHδx
∂ 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 \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δtHδx=limδθ0δθuT(R^Exp(δθ)pl+t^)uT(R^pl+t^)=limδθ0δθuT(R^Exp(δθ)R^)pllimδθ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=\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=[uiT0uiTR^(pli×)000]H=h1h2hM

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) vN(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δxN(0,R)

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

先验分布

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

令真实值为 R R R,对应的李代数为 ϕ \phi ϕ;预测值为 R ^ \hat{R} R^,对应的李代数为 ϕ ^ \hat\phi ϕ^;误差状态为 δ R \delta R δR,对应的李代数为 δ θ \delta\theta δθ;第 k k k次迭代的预测值为 R ^ k \hat R_k R^k,对应的李代数为 ϕ ^ k \hat\phi_k ϕ^k;误差为 δ θ k \delta\theta_k δθ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 ) ) \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} δθ=RR^=R^kδθkR^=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 δθA1(ϕ^kϕ^)δθk+ϕ^kϕ^

右乘BCH公式:
L o g ( E x p ( ϕ 1 ) E x p ( ϕ 2 ) ) ≈ A − 1 ( ϕ 1 ) ϕ 2 + ϕ 1 , ϕ 2 为 小 量 A ( ϕ ) = I + s i n ( ∣ ∣ ϕ ∣ ∣ ) ∣ ∣ ϕ ∣ ∣ ( ϕ × ) 2 ∣ ∣ ϕ ∣ ∣ 2 − 1 − c o s ( ∣ ∣ ϕ ∣ ∣ ) ∣ ∣ ϕ ∣ ∣ ϕ × ∣ ∣ ϕ ∣ ∣ 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))A1(ϕ1)ϕ2+ϕ1,ϕ2A(ϕ)=I+ϕsin(ϕ)ϕ2(ϕ×)2ϕ1cos(ϕ)ϕϕ×
注:Fast-LIO中给出的A阵是左乘BCH近似雅克比矩阵,右乘雅克比矩阵自变量取负数或者转置即可得到左乘雅克比矩阵(所以论文里的A阵都进行了转置)
A l ( ϕ ) = A r ( − ϕ ) 或 者 A l ( ϕ ) = A r T ( ϕ ) 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δθ=A1(ϕ^kϕ^)。令 { 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,A1(ϕ^kϕ^),I,I,I}d=x^kx^,则:
δ x = d + J k δ x k ∼ N ( 0 , P ) \begin{aligned} \delta x&=d+J_k\delta x_k\\ &\sim N(0,P) \end{aligned} δx=d+JkδxkN(0,P)

求解最大后验估计

已知条件概率密度函数 p ( δ z ∣ δ x ) p(\delta z|\delta x) p(δzδx)和先验概率密度函数 p ( δ x ) p(\delta x) p(δx),条件概率分布和先验分布 { z + H δ x k ∼ N ( 0 , R ) d + J k δ x k ∼ 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δxkN(0,R)d+JkδxkN(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)
最大后验估计定义为:求解 δ x k \delta x_k δxk,使得 p ( δ x ∣ δ z ) p(\delta x|\delta z) p(δxδ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)TR1(zk+Hδxk)21(d+Jkδxk)TP1(d+Jkδxk)} minδxk{21(zk+Hδxk)TR1(zk+Hδxk)+21(d+Jkδxk)TP1(d+Jkδxk)} minδxkd+JkδxkP12+zk+HδxkR12
将目标函数 ϵ \epsilon ϵ表示如下:
ϵ = 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 ) \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} ϵ=21d+JkδxkP12+21zk+HδxkR12=21(d+Jkδxk)TP1(d+Jkδxk)+21(zk+Hδxk)TR1(zk+Hδxk)

δ x k \delta x_k δxk的偏导,得到:
∂ ϵ ∂ δ x k = ( J T P − 1 J + H T R − 1 H ) δ x k + J T P − 1 d + H T R − 1 z \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ϵ=(JTP1J+HTR1H)δxk+JTP1d+HTR1z
∂ ϵ ∂ δ x k = 0 \frac{\partial\epsilon}{\partial\delta x_k}=0 δxkϵ=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) \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=(JTP1J+HTR1H)1(JTP1dHTR1z)(1)
Q = ( J T P − 1 J + H T R − 1 H ) − 1 Q=(J^TP^{-1}J+H^TR^{-1}H)^{-1} Q=(JTP1J+HTR1H)1,由矩阵求逆定理,即 ( A − 1 + B D − 1 C ) − 1 = A − A B ( D + C A B ) − 1 C A (A^{-1}+BD^{-1}C)^{-1}=A-AB(D+CAB)^{-1}CA (A1+BD1C)1=AAB(D+CAB)1CA,得到:
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 Q=(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(JTP1J)1HT(H(JTP1J)1HT+R)1H)(JTP1J)1
K = ( J T P − 1 J ) − 1 H T ( H ( J T P − 1 J ) − 1 H T + R ) − 1 K=(J^TP^{-1}J)^{-1}H^T(H(J^TP^{-1}J)^{-1}H^T+R)^{-1} K=(JTP1J)1HT(H(JTP1J)1HT+R)1,此即卡尔曼增益 Q Q Q可表示为:
Q = ( I − K H ) ( J T P − 1 J ) − 1 (2) Q=(I-KH)(J^TP^{-1}J)^{-1}\tag{2} Q=(IKH)(JTP1J)1(2)
U = ( J T P − 1 J ) − 1 U=(J^TP^{-1}J)^{-1} U=(JTP1J)1,联立 { K = U H T ( H U H T + R ) − 1 Q = ( I − K H ) U \begin{cases} K=UH^T(HUH^T+R)^{-1}\\ Q=(I-KH)U \end{cases} {K=UHT(HUHT+R)1Q=(IKH)U,得到:
Q = K R H − T (3) Q=KRH^{-T}\tag{3} Q=KRHT(3)
{ Q = ( I − K H ) ( J T P − 1 J ) − 1 Q = K R H − T \begin{cases}Q=(I-KH)(J^TP^{-1}J)^{-1}\\Q=KRH^{-T}\end{cases} {Q=(IKH)(JTP1J)1Q=KRHT带入 ( 1 ) (1) (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 H − T ( − H T R − 1 z ) = − K z − ( I − K H ) J − 1 d \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=(JTP1J+HTR1H)1(JTP1dHTR1z)=Q(JTP1dHTR1z)=(IKH)(JTP1J)1(JTP1d)+KRHT(HTR1z)=Kz(IKH)J1d

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

卡尔曼增益变形

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

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

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 \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(R1R1H(P1+HTR1H)1HTR1)=(PHTP(P1+HTR1H)I) PHTR1H(P1+HTR1H)1HT)R1=(PHTPHT+(P1+HTR1H)1HT)R1=(P1+HTR1H)1HTR1

参考

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

你可能感兴趣的:(移动机器人,slam,lio,迭代卡尔曼滤波,激光惯导里程计,fastlio)