本文以二维为例,讲解惯性导航(IMU与GPS传感器融合)。
一,坐标系与数据
如下图所示为GPS的轨迹图,(运行utility/showgps.m), 横坐标x为东向,纵坐标为北向.
如下图所示为imu的波形图,可以看出50s处(从上图中看出,车辆右转),角速度为负,局部坐标系应是x朝右,y超前,顺时针为负.
二,惯性导航方程
惯性导航的三大优势:
- 不受外界干扰
- 高频
- 6自由度估计
惯性导航方程描述的是估计变量与IMU量测值之间的关系,首先将加速度计观测到的局部观测值,转换到全局坐标系下去:
a
G
=
R
a
L
a^G = Ra^L
aG=RaL
其中
a
L
a^L
aL是2D的局部加速度,R是姿态.陀螺仪的观测值角速度并没有全局局部之分,直接可以使用时间,将角度的增量求出来:
δ
θ
=
(
y
a
w
r
a
t
e
+
y
a
w
b
i
a
s
)
∗
δ
t
\delta \theta = (yaw_{rate} + yaw_{bias})*\delta t
δθ=(yawrate+yawbias)∗δt
- 姿态的更新
θ t = θ t − 1 + δ θ \theta_t = \theta_{t-1} + \delta \theta θt=θt−1+δθ - 速度的更新
v t = v t − 1 + a G δ t = v t − 1 + R ∗ a L δ t v_t = v_{t-1} + a^G\delta t = v_{t-1} + R*a^L\delta t vt=vt−1+aGδt=vt−1+R∗aLδt - 位置的更新
s t = s t − 1 + v t − 1 δ t + 1 / 2 a G δ t 2 = s t − 1 + v t − 1 δ t + 1 / 2 R ∗ a L δ t 2 s_t = s_{t-1} + v_{t-1}\delta t + 1/2 a^G \delta t^2 = s_{t-1} + v_{t-1}\delta t + 1/2 R*a^L \delta t^2 st=st−1+vt−1δt+1/2aGδt2=st−1+vt−1δt+1/2R∗aLδt2 - bias的更新
b i a s t = b i a s t − 1 bias_{t} = bias_{t-1} biast=biast−1
其中变量维度:姿态,速度,位置,陀螺仪bias, 维度为1+2+2+1共6维
三,Prediction in EKF
状态的更新直接使用惯性导航方程
x
=
[
θ
,
v
,
s
,
b
i
a
s
]
T
x = [\theta, v, s, bias]^T
x=[θ,v,s,bias]T
x
ˉ
(
t
)
=
f
(
x
(
t
−
1
)
)
+
u
\bar{x}(t) = f(x(t-1)) + u
xˉ(t)=f(x(t−1))+u
P
ˉ
=
F
T
P
F
+
Q
\bar{P} = F^TPF + Q
Pˉ=FTPF+Q
其中P是一个66的协方差矩阵,f函数的输入是61, 输出为61, u是一个0为均值,Q为方差的过程噪声.F是一个66的雅克比矩阵:
F
=
∂
f
∂
x
=
[
1
0
0
0
0
δ
t
[
−
sin
(
θ
)
−
cos
(
θ
)
cos
(
θ
)
−
sin
(
θ
)
]
a
L
δ
t
1
0
0
0
0
.
.
.
0
1
0
0
0
1
/
2
[
−
sin
(
θ
)
−
cos
(
θ
)
cos
(
θ
)
−
sin
(
θ
)
]
a
L
δ
t
2
I
2
∗
2
δ
t
.
.
.
I
2
∗
2
.
.
.
0
0
0
0
0
0
1
]
F = \frac{\partial f}{\partial x } \\ = \left[ \begin{matrix}1 & 0 &0 &0 &0&\delta t \\ \left[ \begin{matrix}-\sin(\theta)& -\cos(\theta)\\ \cos(\theta)&-\sin(\theta)\end{matrix}\right ]a^L\delta t &1&0&0&0&0\\ ...&0&1&0&0&0\\ 1/2 \left[ \begin{matrix}-\sin(\theta)& -\cos(\theta)\\ \cos(\theta)&-\sin(\theta)\end{matrix}\right ]a^L\delta t^2 &I_{2*2}\delta t &...&I_{2*2}&...&0\\ 0 & 0 &0 &0 &0&1 \end{matrix}\right ]
F=∂x∂f=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎡1[−sin(θ)cos(θ)−cos(θ)−sin(θ)]aLδt...1/2[−sin(θ)cos(θ)−cos(θ)−sin(θ)]aLδt20010I2∗2δt0001...0000I2∗20000...0δt0001⎦⎥⎥⎥⎥⎥⎥⎥⎥⎤
四,Update using GPS
在组合惯性导航中,GPS是一个非常理想的可以与IMU配合使用的器件.IMU以100hz输出信息,GPS以10HZ输出信息.具体的,在EKF中,每次GPS的观测作为measurement包含到整体系统中.更新三板斧:计算残差,计算卡尔曼增益,计算状态量与方差.
首先残差可以定义为:
y
=
z
−
H
x
ˉ
y = z - H\bar{x}
y=z−Hxˉ
其中z为gps的21维的观测值,
x
ˉ
\bar{x}
xˉ为状态向量.H为26观测矩阵:
H
=
[
0
0
0
1
0
0
0
0
0
0
1
0
]
H = \left[ \begin{matrix}0&0&0&1&0&0\\0&0&0&0&1&0\end{matrix}\right]
H=[000000100100]
卡尔曼增益(5*2)如下:
K
=
P
H
T
(
H
P
H
T
+
R
)
−
1
K = PH^T(HPH^T + R)^{-1}
K=PHT(HPHT+R)−1
计算状态量:
x
=
x
ˉ
+
K
y
x = \bar{x} + Ky
x=xˉ+Ky
计算方差
P
=
(
I
−
K
H
)
P
ˉ
P = (I - KH)\bar{P}
P=(I−KH)Pˉ
五,实验
状态转换函数f(x)以及其雅克比F定义如下:
theta_t = theta_t_1 + yawrate*delta_t;
v_t = v_t_1 + to_R2d(theta_t_1)*a_L*delta_t;
s_t = s_t_1 + v_t_1*delta_t+ 1/2*to_R2d(theta_t_1)*a_L*delta_t*delta_t;
fx =[theta_t;v_t;s_t];
dR_theta = [-sin(theta_t_1),-cos(theta_t_1);cos(theta_t_1),-sin(theta_t_1)];
F = eye(5,5);
F(2:3, 1) = dR_theta*a_L*delta_t;
F(4:5, 1) =1/2*dR_theta*a_L*delta_t*delta_t;
F(4:5, 2:3) = eye(2,2)*delta_t;
预测部分:
[X_bar, F] = get_state_transition_F(delta_t,yaw_rate,aL,X(1),X(2:3),X(4:5));
Q = get_Q();
P_bar = F'*P*F + Q;
更新部分:
H = zeros(2,5);
H(1:2,4:5) = eye(2,2);
R = 4*eye(2,2);
z = data2d.GNSS.pos_EN(:,it_gps);
y = z - H*X_bar;
K = P_bar*H'*inv(H*P_bar*H' + R);
X = X_bar + K*y;
P = (eye(5,5) - K*H)*P_bar;
实验得到如下位置结果:
可以看出方向大致正确,因为GPS 1HZ的原因,导致在接收GPS的瞬间,并不是特别连续.
代码见:github 2d case. run: run_kf_INS.m.
接着使用tutlebot仿真数据,该数据有三个优点:1,IMU数据漂移较小。2,GPS为10HZ,3,坐标轴无歧义。
全局轨迹结果如下,其中红色为gps观测,蓝色为imu积分轨迹
将一处转弯处放大的结果如下: