目录
0 前言
在很多工程应用里都需要获得物体的姿态信息,而通过imu估计姿态应该最广。
仅使用imu获取姿态信息有很多算法,本文讨论基于ESKF(Error-State Kalman Filter)的算法。
本文偏向于算法的工程应用,因此省去了公式推导,但是会列出必不可少的核心公式。
1 什么是ESKF
ESKF(Error-State Kalman Filter)看起来很吓唬人,其实它和一般的KF、EKF没有什么本质的区别,只是状态量的选取不同罢了。
以位姿估计为例,普通的kalman filter的状态量(名义状态量)一般都是位置p、速度v、四元数q,而ESKF的状态量(误差状态量)是上述状态量的误差,如位置的误差δp、速度误差δv、姿态误差δq,而kalman得5个核心公式是没有区别的。
为了方便阅读,kalman的核心公式扔在这里。
优点(不翻译了)
2 系统方程
2.1 状态变量
1)真实状态量:
x
t
=
[
q
t
ω
b
t
]
x_t= \begin{bmatrix} q_t \\ \omega_{bt} \end{bmatrix}
xt=[qtωbt],
q
t
q_t
qt为真实四元数,
ω
b
t
\omega_{bt}
ωbt为陀螺仪的真实bias。
2)状态量(名义状态量):
x
=
[
q
ω
b
]
x= \begin{bmatrix} q \\ \omega_b \end{bmatrix}
x=[qωb],
q
q
q为四元数,
ω
b
\omega_{b}
ωb为陀螺仪的bias。
3)误差状态量:
δ
x
=
[
δ
q
δ
ω
b
]
\delta x= \begin{bmatrix} \delta q \\ \delta\omega_b \end{bmatrix}
δx=[δqδωb],
δ
q
\delta q
δq、
δ
ω
b
\delta\omega_b
δωb分别真实状态量和名义状态量的误差。
那么他们之间有这样的关系:
x
t
=
x
⊕
δ
x
=
[
q
×
δ
q
ω
b
+
δ
ω
b
]
x_t = x\oplus\delta x= \begin{bmatrix} q\times \delta q \\ \omega_b + \delta\omega_b \end{bmatrix}
xt=x⊕δx=[q×δqωb+δωb]
其中,
δ
q
\delta q
δq和
δ
θ
\delta\theta
δθ的关系为:
δ
q
=
e
δ
θ
/
2
\delta q=e^{\delta\theta/2}
δq=eδθ/2,因为有
q ≜ E x p ( ϕ u ) = e ϕ u / 2 = c o s ( ϕ / 2 ) + u s i n ( ϕ / 2 ) = [ c o s ( ϕ / 2 ) u s i n ( ϕ / 2 ) ] q\triangleq Exp(\phi \boldsymbol{u}) =e^{\phi \boldsymbol{u}/2} = cos (\phi/2) + \boldsymbol{u}sin(\phi/2) = \begin{bmatrix} cos(\phi/2) \\ \boldsymbol{u}sin(\phi/2) \end{bmatrix} q≜Exp(ϕu)=eϕu/2=cos(ϕ/2)+usin(ϕ/2)=[cos(ϕ/2)usin(ϕ/2)]
也就是常用的轴角公式,由于
δ
θ
\delta\theta
δθ只需要3个变量表示,所以误差状态量又可以表示为更简洁的形式:
δ
x
=
[
δ
θ
δ
ω
b
]
\delta x= \begin{bmatrix} \delta \theta \\ \delta\omega_b \end{bmatrix}
δx=[δθδωb]
2.2 imu的测量值
1)加速度计
a
t
=
a
m
+
a
n
a_t=a_m+a_n
at=am+an
其中
a
t
a_t
at为真值,
a
m
a_m
am为测量值,
a
n
a_n
an为测量噪声。
2)陀螺仪
ω
t
=
ω
m
+
ω
n
\omega_t = \omega_m + \omega_n
ωt=ωm+ωn
其中
ω
t
\omega_t
ωt为真值,
ω
m
\omega_m
ωm为测量值,
ω
n
\omega_n
ωn为测量噪声。
2.3 预测方程及雅克比矩阵
这里我们是用陀螺仪的数据对姿态进行预测。
对于名义状态量,有:
{
q
˙
=
1
2
q
⊗
(
ω
m
−
ω
b
)
ω
˙
b
=
0
\left \{ \begin{array}{ll} \dot{q} = \frac{1}{2}q \otimes(\omega_m - \omega_b) \\ \dot{\omega}_b = 0 \end{array} \right.
{q˙=21q⊗(ωm−ωb)ω˙b=0
对于误差状态量,有:
{
δ
θ
˙
=
−
[
ω
m
−
ω
b
]
×
δ
θ
−
δ
ω
b
−
ω
n
δ
ω
b
˙
=
ω
ω
\left \{ \begin{array}{ll} \dot{\delta \theta} = -[\omega_m - \omega_b]_{\times}{\delta \theta} - \delta{\omega_b} - \omega_n \\ \dot{\delta \omega _b} = \omega_{\omega} \\ \end{array} \right.
{δθ˙=−[ωm−ωb]×δθ−δωb−ωnδωb˙=ωω
上式中,
ω
n
\omega_n
ωn为陀螺仪数据噪声,
ω
ω
\omega_{\omega}
ωω为陀螺仪零偏的噪声(仔细品味一下,象征陀螺仪零偏的稳定性);
[
∙
]
×
[\bullet]_{\times}
[∙]×为反对称矩阵。
这里,我们把误差状态的系统方程的一般形式表示为:
δ
x
←
f
(
x
,
δ
x
,
u
m
,
i
)
=
F
x
(
x
,
u
m
)
⋅
δ
x
+
F
i
⋅
i
\delta x \gets f(x,\delta x, u_m, i) = F_x(x, u_m) \cdot \delta x + F_i \cdot i
δx←f(x,δx,um,i)=Fx(x,um)⋅δx+Fi⋅i
上式中,
u
m
u_m
um为系统的输入,
i
i
i为噪声。
那么其预测方程可以写为:
δ
x
^
←
F
x
(
x
,
u
m
)
⋅
δ
x
^
\hat{\delta x} \gets F_x(x, u_m) \cdot \hat{\delta x}
δx^←Fx(x,um)⋅δx^
P
←
F
x
P
F
x
T
+
F
i
Q
i
F
i
T
P \gets F_xPF^T_x + F_iQ_iF^T_i
P←FxPFxT+FiQiFiT
其中:
F
x
=
∂
f
∂
δ
x
∣
x
,
u
m
=
[
R
T
{
(
ω
m
−
ω
b
)
Δ
t
}
−
I
Δ
t
0
I
]
F_x = \frac{\partial f}{\partial \delta x} \lvert_{x,u_m} = \begin{bmatrix} R^T\{{(\omega_m-\omega_b)\Delta t}\} & -I\Delta t \\ 0 & I \end{bmatrix}
Fx=∂δx∂f∣x,um=[RT{(ωm−ωb)Δt}0−IΔtI]
上式中,
R
{
u
}
=
I
+
s
i
n
ϕ
[
u
]
×
+
(
1
−
c
o
s
ϕ
)
[
u
]
×
2
R\{\boldsymbol{u}\} = I + sin \phi[\boldsymbol{u}]_{\times} + (1-cos \phi)[\boldsymbol{u}]^2_{\times}
R{u}=I+sinϕ[u]×+(1−cosϕ)[u]×2,有
[
u
]
×
2
=
u
u
T
−
I
[\boldsymbol{u}]^2_{\times} = \boldsymbol{u}\boldsymbol{u}^T-I
[u]×2=uuT−I
F
i
=
∂
f
∂
i
∣
x
,
u
m
=
[
I
0
0
I
]
F_i = \frac{\partial f}{\partial i} \lvert_{x,u_m}= \begin{bmatrix} I & 0 \\ 0&I \end{bmatrix}
Fi=∂i∂f∣x,um=[I00I]
Q i = [ σ ω n 2 Δ t 2 I 0 0 σ ω ω 2 Δ t I ] Q_i = \begin{bmatrix} \sigma^2_{\omega_n}\Delta t^2 I & 0 \\ 0 & \sigma^2_{\omega_{\omega}}\Delta t I \end{bmatrix} Qi=[σωn2Δt2I00σωω2ΔtI]
需要注意的是,
δ
x
\delta x
δx不需要做预测,因为每次初始化
δ
x
\delta x
δx都为0,所以预测值肯定也是0。
有人会问,它会一直是0吗,经过几次运算迭代一直是0?
如果有这样的疑问,那就说明还没有从常规kalman filter的模式中走出来。ESKF每次迭代后,都会将最优估计得到的
δ
x
\delta x
δx叠加到
x
x
x中,所以对于下一时刻的开始,认为
x
x
x是没有误差的,也就是认为
δ
x
\delta x
δx为0了。
总结,在kalman预测这一步,每一轮新的kalman迭代,需要做的事情是:
1)对名义状态量进行预测;
3)计算雅克比矩阵
2)对误差状态量进行预测;( δ x \delta x δx的值是0,可以不计算,但是不能认为它没有意义)
2.4 测量方程及雅克比矩阵
观测方程可以表示为下面的形式
y
=
h
(
x
t
)
+
v
y=h(x_t)+v
y=h(xt)+v
这里使用imu的加速度进行姿态的校正,假设的前提是没有机动加速度,只有重力加速度,所以有
y
=
a
m
=
−
R
t
T
g
+
a
n
y=a_m=-R^T_t \boldsymbol{g} + a_n
y=am=−RtTg+an,其中
R
t
T
R_t^T
RtT是通过
q
t
q_t
qt得到的旋转矩阵的转置。
带入
R
T
R^T
RT,可以把上式化简为:
y
=
h
(
x
t
)
+
v
=
−
R
t
T
g
+
a
n
=
−
R
t
T
[
0
0
g
]
+
a
n
=
[
2
(
q
x
q
z
−
q
w
q
y
)
2
(
q
y
q
z
+
q
w
q
x
)
q
w
2
−
q
x
2
−
q
y
2
+
q
z
2
]
(
−
g
)
+
a
n
\begin{aligned} y & =h(x_t)+v \\ & = -R^T_t \boldsymbol{g} + a_n \\ & = -R^T_t \begin{bmatrix} 0\\0\\g \end{bmatrix}+ a_n =\begin{bmatrix} 2(q_xq_z-q_wq_y) \\ 2(q_yq_z+q_wqx) \\ q^2_w -q^2_x-q^2_y+q^2_z \end{bmatrix} (-g) + a_n \end{aligned}
y=h(xt)+v=−RtTg+an=−RtT⎣⎡00g⎦⎤+an=⎣⎡2(qxqz−qwqy)2(qyqz+qwqx)qw2−qx2−qy2+qz2⎦⎤(−g)+an
然后将
h
(
x
t
)
h(x_t)
h(xt)对
δ
x
\delta x
δx求偏导,即
H
≜
∂
h
∂
δ
x
∣
x
=
∂
h
∂
x
t
∣
x
∂
x
t
∂
δ
x
∣
x
=
H
x
X
δ
x
H \triangleq \frac{\partial h}{\partial \delta x}|_x = \frac{\partial h}{\partial x_t}|_x \frac{\partial x_t}{\partial \delta x}|_x = H_x X_{\delta x}
H≜∂δx∂h∣x=∂xt∂h∣x∂δx∂xt∣x=HxXδx
H x = ∂ h ∂ δ x ∣ x = [ − 2 q y 2 q z − 2 q w 2 q x 2 q x 2 q w 2 q z 2 q y 2 q w − 2 q x − 2 q y 2 q z ] ( − g ) H_x = \frac{\partial h}{\partial \delta x}|_x = \begin{bmatrix} -2q_y & 2q_z & -2q_w & 2q_x \\ 2q_x & 2q_w & 2q_z & 2q_y \\ 2q_w & -2q_x & -2q_y & 2q_z \end{bmatrix} (-g) Hx=∂δx∂h∣x=⎣⎡−2qy2qx2qw2qz2qw−2qx−2qw2qz−2qy2qx2qy2qz⎦⎤(−g)
X δ x = [ ∂ ( q ⊗ δ q ) ∂ δ θ 0 0 ∂ ( ω b + δ ω b ) ∂ δ ω b ] = [ Q δ θ 0 0 I ] X_{\delta x} = \begin{bmatrix} \frac{\partial(q \otimes \delta q)}{\partial \delta \theta} & 0 \\ 0 & \frac{\partial(\omega_b + \delta \omega _b)}{\partial \delta \omega _b} \end{bmatrix} = \begin{bmatrix} Q_{\delta_\theta} & 0 \\ 0 & I \end{bmatrix} Xδx=[∂δθ∂(q⊗δq)00∂δωb∂(ωb+δωb)]=[Qδθ00I]
Q δ θ = 1 2 [ − q x − q y − q z q w − q z q y q z q w − q x − q y q x q w ] Q_{\delta \theta} = \frac{1}{2} \begin{bmatrix} -q_x & -q_y & -q_z \\ q_w & -q_z &q_y \\ q_z & q_w & -q_x \\ -q_y & q_x & q_w \end{bmatrix} Qδθ=21⎣⎢⎢⎡−qxqwqz−qy−qy−qzqwqx−qzqy−qxqw⎦⎥⎥⎤
3 kalman filter loop计算
上面的公式太多了,看得人头皮发麻,那就赶紧进入算法实践环节,接下来就可以按照Kalman Filter的“套路”计算了,大致的流程如下。
上图中的每一个公式或者变量都可以在前文中找到定义和说明,你要做的就是按照上述的流程进行计算就行。
4 Show me the code
有人说,没有代码代码,还是太抽象,好吧,这里奉上MATLAB脚本和仿真数据。
代码文件的结构如下。
仿真结果如下。
%% 使用IMU的数据,基于ESKF的方法估计姿态
clc;clear;close;
%% 添加路径,加载数据
addpath('ESKF')
load('imu_log_data.mat');
mean_dt = mean(diff(imu.t));
%% 调用算法,记录数据
eskf.t = imu.t;
[eskf.quat, eskf.gyb, eskf.cov] = eskf_imu(imu.t, imu.gyr, imu.acc);
[eskf.yaw, eskf.pitch, eskf.roll] = quat2angle(eskf.quat,'ZYX');
%% 画图
figure('Name', 'euler in degree');
h1 = subplot(3,1,1);
plot(eskf.t, rad2deg(eskf.roll), 'r');
grid minor;title('roll')
h2 = subplot(3,1,2);
plot(eskf.t, rad2deg(eskf.pitch), 'r');
grid minor;title('pitch')
h3 = subplot(3,1,3);
plot(eskf.t, rad2deg(eskf.yaw), 'r');
grid minor;title('yaw')
linkaxes([h1 h2 h3],'x');
function [state_quat, state_gyb, state_cov] = eskf_imu(t, gyr, acc)
%% 参数初始化
quat = quaternion(1,0,0,0);
init_quat_var = power(4, 2);
init_gyb_var = power(1e-3, 2);
% 初始化P矩阵
P(1:3,1:3) = init_quat_var * eye(3);
P(4:6,4:6) = init_gyb_var * eye(3);
acc_meas_noise_var = power(2, 2); % 加速度计噪声方差
R = eye(3) * acc_meas_noise_var;
gyr_noise = power(1e-2, 2);
gyr_drift_noise = power(1e-3, 2);
gyb = zeros(1,3);
%% 申请内存,可以加快仿真速度
len = length(gyr);
state_quat = zeros(len,4);
state_gyb = zeros(len,3);
state_cov = zeros(len,6);
%% 循环计算
for index = 1:len
if (index == 1)
dt = saturate_user(t(2) - t(1),0.001,0.05);
else
dt = saturate_user(t(index) - t(index-1),0.001,0.05);
end
% ---- 预测 ----- %
% 预测姿态角
d_angle = (gyr(index,:) - gyb) * dt;
dq = quaternion(d_angle, 'rotvec'); % 将旋转向量转化为四元数
new_quat = quat * dq;
quat = new_quat.normalize;
% 预测协方差
P = ESKF_predict_P(dt, P, gyr_noise, gyr_drift_noise);
% ---- 更新 ----- %
y = ESKF_predict_acc(quat); %
H = ESKF_jacH(quat);
S = H * P * H' + R;
K = P * H' / S;
P = P - K * H * P;
innov = K * (acc(index,:) - y)';
dq = quaternion(innov(1:3)', 'rotvec');
new_quat = quat * dq;
quat = new_quat.normalize;
gyb = gyb + innov(4:6)';
state_quat(index,:) = quat.compact;
state_gyb(index,:) = gyb;
state_cov(index,:) = diag(P)';
end
end
function out = saturate_user(in, min, max)
if (in < min)
out = min;
elseif (in > max)
out = max;
else
out = in;
end
end
function new_P = ESKF_predict_P(dt, P, gyro_noise, gyr_drift_noise)
Fx = eye(6);
Fx(1:3,4:6) = -eye(3) * dt;
Fi = eye(6);
Qi(1:3, 1:3) = eye(3) * gyro_noise * dt * dt;
Qi(4:6, 4:6) = eye(3) * gyr_drift_noise * dt;
new_P = Fx * P * transpose(Fx) + Fi * Qi * transpose(Fi);
end
function pred_meas = ESKF_predict_acc(quat)
[qw, qx, qy, qz] = parts(quat);
gravity = 9.80665;
pred_meas(1) = -gravity * 2 * (qx * qz - qw * qy);
pred_meas(2) = -gravity * 2 * (qw * qx + qy * qz);
pred_meas(3) = -gravity * (qw * qw - qx * qx - qy * qy + qz * qz);
end
function H = ESKF_jacH(quat)
[qw, qx, qy, qz] = parts(quat);
Hx = zeros(3,7);
Xdx = zeros(7,6);
Hx(1:3,1:4) = -9.80665 * 2 * [ -qy, qz, -qw, qx;...
qx, qw, qz, qy;...
qw, -qx, -qy, qz];
Xdx(1:4,1:3) = 0.5 * [-qx, -qy, -qz;...
qw, -qz, qy;...
qz, qw, -qx;...
-qy, qx, qw];
Xdx(5:7,4:6) = eye(3);
H = Hx * Xdx;
end
如果还有什么不清楚,欢迎评论区交流。
5 代码下载链接
可以在这里下载。链接: 点击下载。