写在前面
这次来总结一下IMU的一些东西,主要包含:
IMU的测量模型
通常,我们将IMU的测量模型写作如下模型(注意不是观测模型):
wm=wt+bw+nwam=at+ba+na
亦即:“测量值=真值+零偏+高斯白噪声”,其中:
- 零偏值是我们希望实时进行估计的,会在状态变量中进行估计;
- 零偏值通常被建模为随机游走过程;
高斯白噪声
高斯白噪声其实是我们常见的噪声模型,一个典型的高斯白噪声满足:
E[n(t)]=0E[n(t1)n(t2)]=σg2δ(t1−t2)
如果一个传感器的噪声仅仅包含高斯白噪声(或者白噪声)的话,那么基本上,使用平均滤波是能得到较好的有效值的,因为白噪声的期望值是0;
下一步把连续过程离散化,可以比较直观的得到:nd(k)=σgdw(k),其中σd是高斯白噪声的方差,而w(k)为标准的正态分布。
值得说明的一点是:σgd与连续空间的方差并不相同,而是等于σg/Δt,直接去想的话,可以认为因为离散空间的积分与时间间隔相关,每次积分都要乘以Δt,所以最终的方差多了一个Δt,所以要除下来;这里还是给出数学上的推导:
nd[k]E(nd[k]2)≜n(t0+Δt)≃Δt1∫t0t0+Δtn(τ)dt=E(Δt21∫t0t0+Δt∫t0t0+Δtn(τ)n(t)dτdt)=E(Δt2σg2∫t0t0+Δt∫t0t0+Δtδ(t−τ)dτdt)=E(Δtσg2)
一开始我比较不理解第一行的公式,但是后来仔细想想,其实一个时刻的高斯噪声值可以看做是某个时间段内所有值的平均。需要注意的是,高斯白噪声由概率密度描述的,在Kalibr的配置文件中,需要输入加计和陀螺仪的”噪声密度“值,对应σgd。
Bias的随机游走
随机游走过程本质上是离散的,大家可以想象一些折线图,每个拐点就是那一时刻的值;其在连续空间被建模为一个维纳过程,也就是导数为高斯白噪声的过程:
bg˙=n(t)
根据上面得到的高斯白噪声模型,随机游走的离散模型就比较简单了:
bg(k+1)=bg(k)+σgdw(t)Δt=bg(k)+σgΔtw(t)
在Kalibr的配置文件中,需要输入加计和陀螺仪的随机游走的方差值,也就是上面的σgΔt。
IMU运动模型
说起运动模型,首先就要确立状态变量,IMU的状态变量比较多,为如下形式:
X=[WPIT,WVIT,qIWT,baT,bwT]T
除了正常的位姿之外,IMU的模型中添加了速度和零偏的估计值,对应的运动模型为:
⎩⎪⎪⎪⎪⎨⎪⎪⎪⎪⎧Wp˙=WvWv˙=RIW(Iam−ba−na)q˙IW=qIW⊗q(Iwm−bw−nw)b˙a=nadb˙w=nwd
注意上式的p,v都在参考坐标系W下,而旋转使用四元数表示,关于四元数更多的内容,可以查看here
reference
https://fzheng.me/2016/11/20/imu_model_eq/
https://github.com/ethz-asl/kalibr/wiki/IMU-Noise-Model