惯性导航(三)估计偏置

文章目录

    • 一、状态时空模型
      • 1、状态向量定义、预测模型
      • 2、协方差部分
    • 二、量测更新部分
    • 三、实验结果
      • 1. (加速度bias)真实数据 a b x = 0.0 a_b^x = 0.0 abx=0.0, 初始值也为 a b x = 0.0 a_b^x = 0.0 abx=0.0;
      • 2. (加速度bias)真实数据 a b x = 0.0 a_b^x = 0.0 abx=0.0,初始为 a b x = − 0.1 a_b^x = -0.1 abx=0.1.
      • 3. (加速度bias)真实数据 a b = [ 0.1 , 0.1 ] T a_b = [0.1, 0.1]^T ab=[0.1,0.1]T,初始为 a b x = [ 0 , 0 ] T a_b^x = [0,0]^T abx=[0,0]T.
      • 4. (角速度bias)真实数据 θ b = 0 \theta_b =0 θb=0, 初始值为 θ b = − 0.03 \theta_b =-0.03 θb=0.03
    • 参考文献

真实工况下IMU都会有较大的偏置,偏置一般与温度、出厂精度、实际运动动态程度。在位姿估计中,会有累积效应,因此估计偏置是惯性导航中的常用做法。

一、状态时空模型

1、状态向量定义、预测模型

惯性导航方程描述的是估计变量与IMU量测值之间的关系,首先将加速度计观测到的局部观测值,转换到全局坐标系下去:
a G = R ( a L + a b ) a^G = R(a^L + a_b) aG=R(aL+ab)
其中 a L a^L aL是2D的局部加速度, a b a_b ab是偏置,R是姿态.陀螺仪的观测值角速度并没有全局局部之分,直接可以使用时间,将角度的增量求出来:
δ θ = ( θ r a t e + θ b ) ∗ δ t \delta \theta = (\theta_{rate} + \theta_{b})*\delta t δθ=(θrate+θb)δt

  • 位置的更新
    p t = p t − 1 + v t − 1 δ t + 1 / 2 a G δ t 2 p_t =p_{t-1} + v_{t-1}\delta t + 1/2 a^G \delta t^2 pt=pt1+vt1δt+1/2aGδt2
  • 速度的更新
    v t = v t − 1 + a G δ t v_t = v_{t-1} + a^G\delta t vt=vt1+aGδt
  • 姿态的更新
    θ t = θ t − 1 + δ θ \theta_t = \theta_{t-1} + \delta \theta θt=θt1+δθ
  • bias的更新
    a b ( t ) = a b ( t − 1 ) a_{b(t)} = a_{b(t-1)} ab(t)=ab(t1)
    θ b ( t ) = θ b ( t − 1 ) \theta_{b(t)} = \theta_{b(t-1)} θb(t)=θb(t1)
    综上,状态向量组成:PVQ,Bias,一共8维. 根据EKF的标准形式: X ˉ t = f ( X t − 1 , u ) \bar{X}_t = f(X_{t-1},u) Xˉt=f(Xt1,u), X = [ p , v , θ , a b , θ b ] X = [p,v,\theta,a_b,\theta_b] X=[p,v,θ,ab,θb].

2、协方差部分

非线性模型f的对 X t − 1 X_{t-1} Xt1的偏导数形成转换矩阵F.

  • 其关于位置的部分:
    F 1 − 2 = [ 1 0 δ t 0 ∂ p ∂ θ ∂ p ∂ a b ∂ p ∂ θ b 0 1 0 δ t − − − ] F_{1-2} = [\begin{matrix}1 & 0 & \delta t&0&\frac{\partial p}{\partial \theta}&\frac{\partial p}{\partial a_b}&\frac{\partial p}{\partial \theta_b}\\ 0&1&0&\delta t&-&-&-\end{matrix}] F12=[1001δt00δtθpabpθbp]
    其中:
    ∂ p ∂ θ = 0.5 R ′ ( a L + a b ) δ t 2 \frac{\partial p}{\partial \theta} = 0.5R'(a^L+a_b)\delta t^2 θp=0.5R(aL+ab)δt2, ∂ p ∂ a b = 0.5 R [ 1 1 ] δ t 2 \frac{\partial p}{\partial a_b} = 0.5R [\begin{matrix}1\\1\end{matrix}]\delta t^2 abp=0.5R[11]δt2 ∂ p ∂ θ b = 0.5 R ′ ( a L + a b ) δ t 2 ? \frac{\partial p}{\partial \theta_b} =0.5R'(a^L+a_b)\delta t^2? θbp=0.5R(aL+ab)δt2(应该是0), R ′ = [ − s i n θ − c o s θ c o s θ − s i n θ ] R' = [\begin{matrix}-sin\theta& -cos\theta\\cos\theta &-sin\theta\end{matrix}] R=[sinθcosθcosθsinθ]
  • 关于速度部分:
    F 3 − 4 = [ 0 0 1 0 ∂ v t ∂ θ t − 1 ∂ v t ∂ a b 0 0 0 0 1 − − 0 ] F_{3-4} = [\begin{matrix}0 &0&1&0&\frac{\partial v_{t}}{\partial \theta_{t-1}} &\frac{\partial v_t}{\partial a_b} &0 \\0&0&0&1&-&-&0 \end{matrix}] F34=[00001001θt1vtabvt00]
    其中: ∂ v t ∂ θ t − 1 = R ′ ( a L + a b ) δ t \frac{\partial v_{t}}{\partial \theta_{t-1}} =R'(a^L+a_b)\delta t θt1vt=R(aL+ab)δt, ∂ v t ∂ a b = R [ 1 , 1 ] T δ t \frac{\partial v_t}{\partial a_b} = R[1,1]^T\delta t abvt=R[1,1]Tδt.
  • 关于姿态部分:
    F 5 = [ 0 0 0 0 1 0 0 δ t ] F_{5} = [\begin{matrix}0&0&0&0&1&0&0&\delta t\end{matrix}] F5=[0000100δt]
  • 关于加速度/角速度偏置部分:
    F 6 − 8 = [ 0 0 0 0 0 1 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 1 ] F_{6-8} = [\begin{matrix}0&0&0&0&0&1&0&0\\ 0&0&0&0&0&0&1&0\\0&0&0&0&0&0&0&1\end{matrix}] F68=[000000000000000100010001]

二、量测更新部分

这里先引入GPS作为观测信息,根据 y = z − H x ˉ y = z-H\bar{x} y=zHxˉ,可知:
H = [ 1 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 ] H = [\begin{matrix}1&0&0&0&0&0&0&0\\0&1&0&0&0&0&0&0\end{matrix}] H=[1001000000000000]

三、实验结果

实验步骤:

  1. 运行数据生成脚本:/media/m0/Data/code/gnss-ins-sim/demo_no_algo.py,轨迹配置文件/media/m0/Data/code/gnss-ins-sim/demo_motion_def_files/motion_def-2d_long.csv,IMU配置文件/media/m0/Data/code/gnss-ins-sim/gnss_ins_sim/sim/imu_model.py
  2. 运行算法运行脚本:/home/m0/code/INS/2d_case/04_gnss_sim_bias/run_gnss_imu_sim.m

1. (加速度bias)真实数据 a b x = 0.0 a_b^x = 0.0 abx=0.0, 初始值也为 a b x = 0.0 a_b^x = 0.0 abx=0.0;

惯性导航(三)估计偏置_第1张图片可以看出估计出来的bias也为0,符合预期。

2. (加速度bias)真实数据 a b x = 0.0 a_b^x = 0.0 abx=0.0,初始为 a b x = − 0.1 a_b^x = -0.1 abx=0.1.

惯性导航(三)估计偏置_第2张图片从左下角的bias估计可以看出,从-0.1收敛到0附近,符合预期。

3. (加速度bias)真实数据 a b = [ 0.1 , 0.1 ] T a_b = [0.1, 0.1]^T ab=[0.1,0.1]T,初始为 a b x = [ 0 , 0 ] T a_b^x = [0,0]^T abx=[0,0]T.

惯性导航(三)估计偏置_第3张图片可以看出,bias收敛到真值附近。

4. (角速度bias)真实数据 θ b = 0 \theta_b =0 θb=0, 初始值为 θ b = − 0.03 \theta_b =-0.03 θb=0.03

惯性导航(三)估计偏置_第4张图片
可以看到航向角收敛至0,但是也影响到了加速度计的偏置估计,对于协方差的调优将在后续文章中解释。

参考文献

  1. 一个通俗的博客

你可能感兴趣的:(SLAM)