CKF流程
对于k时刻的状态 X ^ k \mathbf{\hat X}_k X^k以及协方差 P k \mathbf{P}_k Pk,令 L = d i m ( X ^ k ) L=dim(\mathbf{\hat X}_k) L=dim(X^k),CKF执行以下两步:
预测
定义矩阵
ξ
\xi
ξ为:
ξ
=
L
[
I
L
−
I
L
]
\xi = \sqrt{L}\begin{bmatrix}\mathbf I_L & -\mathbf I_L\end{bmatrix}
ξ=L[IL−IL]
计算容积点:
S
k
=
P
k
\mathbf S_k=\sqrt{\mathbf P_k}
Sk=Pk,对于第i个采样点,有:
χ
k
i
=
X
^
k
+
S
k
ξ
i
\chi_k^i = \mathbf{\hat X}_k + \mathbf S_k \xi_i
χki=X^k+Skξi
对每个容积点进行状态传播得到k+1时刻的预测状态以及协方差:
χ
k
+
1
∣
k
i
=
f
(
χ
k
i
)
,
i
=
1
,
2
,
.
.
.
,
2
L
X
^
k
+
1
∣
k
=
1
2
L
∑
i
=
1
i
=
2
L
χ
k
+
1
∣
k
i
P
k
+
1
∣
k
=
1
2
L
∑
i
=
1
i
=
2
L
(
χ
k
+
1
∣
k
i
−
X
^
k
+
1
∣
k
)
(
χ
k
+
1
∣
k
i
−
X
^
k
+
1
∣
k
)
T
+
Q
k
+
1
\begin{aligned} \chi_{k+1|k}^i&=f(\chi_k^i ), i = 1,2,...,2L \\ \mathbf{\hat X}_{k+1|k}&=\frac{1}{2L}\sum_{i=1}^{i=2L}\chi_{k+1|k}^i \\ \mathbf P_{k+1|k}&=\frac{1}{2L}\sum_{i=1}^{i=2L}(\chi_{k+1|k}^i- \mathbf{\hat X}_{k+1|k})(\chi_{k+1|k}^i- \mathbf{\hat X}_{k+1|k})^T+\mathbf{Q}_{k+1} \end{aligned}
χk+1∣kiX^k+1∣kPk+1∣k=f(χki),i=1,2,...,2L=2L1i=1∑i=2Lχk+1∣ki=2L1i=1∑i=2L(χk+1∣ki−X^k+1∣k)(χk+1∣ki−X^k+1∣k)T+Qk+1
更新
同样对协方差进行分解得到
S
k
+
1
\mathbf S_{k+1}
Sk+1,进而得到每个容积点
χ
k
+
1
i
\chi_{k+1}^i
χk+1i,并对量测进行预测:
Z
k
+
1
i
=
h
(
χ
k
+
1
i
)
z
^
k
+
1
=
1
2
L
∑
i
=
1
i
=
2
L
Z
k
+
1
i
\begin{aligned} \mathbf{Z}_{k+1}^i&=h(\chi_{k+1}^i) \\ \mathbf{\hat z}_{k+1}&=\frac{1}{2L}\sum_{i=1}^{i=2L}\mathbf{Z}_{k+1}^i \end{aligned}
Zk+1iz^k+1=h(χk+1i)=2L1i=1∑i=2LZk+1i
之后对量测的协方差以及状态-量测互协方差进行计算:
P
k
+
1
z
z
=
1
2
L
∑
i
=
1
i
=
2
L
(
Z
k
+
1
i
−
z
^
k
+
1
)
(
Z
k
+
1
i
−
z
^
k
+
1
)
T
+
R
k
+
1
P
k
+
1
x
z
=
1
2
L
∑
i
=
1
i
=
2
L
(
χ
k
+
1
i
−
X
^
k
+
1
∣
k
)
(
Z
k
+
1
i
−
z
^
k
+
1
)
T
\begin{aligned} \mathbf{P}^{zz}_{k+1}&=\frac{1}{2L}\sum_{i=1}^{i=2L}(\mathbf Z_{k+1}^i- \mathbf{\hat z}_{k+1})(\mathbf Z_{k+1}^i- \mathbf{\hat z}_{k+1})^T+\mathbf{R}_{k+1} \\ \mathbf{P}^{xz}_{k+1}&=\frac{1}{2L}\sum_{i=1}^{i=2L}(\chi_{k+1}^i-\mathbf{\hat X}_{k+1|k})(\mathbf Z_{k+1}^i- \mathbf{\hat z}_{k+1})^T \end{aligned}
Pk+1zzPk+1xz=2L1i=1∑i=2L(Zk+1i−z^k+1)(Zk+1i−z^k+1)T+Rk+1=2L1i=1∑i=2L(χk+1i−X^k+1∣k)(Zk+1i−z^k+1)T
由此得到卡尔曼增益
K
k
+
1
=
P
k
+
1
x
z
(
P
k
+
1
z
z
)
−
1
\mathbf K_{k+1}=\mathbf{P}^{xz}_{k+1}(\mathbf{P}^{zz}_{k+1})^{-1}
Kk+1=Pk+1xz(Pk+1zz)−1,执行正常的滤波更新。
IMU方向估计
ImuFusion提供的orien.m程序中,利用陀螺仪数据做预测,加速度数据做方向更新。将其修改为CKF的滤波形式:
% ImuFusion based on CKF
for k = 2 : N
%--- 1. Propagation --------------------------
% Gyroscope measurements
w = (imu_data(k-1, 2:4) + imu_data(k, 2:4))/2;
w = w - bias_w;
% Compute the F matrix
Omega =[0 -w(1) -w(2) -w(3);...
w(1) 0 w(3) -w(2); ...
w(2) -w(3) 0 w(1); ...
w(3) w(2) -w(1) 0 ];
F = eye(4) + deltat * Omega / 2;
% Compute the process noise Q
G = [-x(2) -x(3) -x(4); ...
x(1) -x(4) x(3); ...
x(4) x(1) -x(2); ...
-x(3) x(2) x(1)] / 2;
% Propagate the state and covariance
S1 = chol(P)';
x_axis = (quat2rod(x'))';
xsigmak = S1*kesai + repmat(x_axis,1,2*L);
for i = 1 : 2*L
xsigmak(:,i) = (quat2rod((F*rod2quat(xsigmak(:,i)')')'))';
end
xpre = sum(xsigmak, 2)/(2*L);
P = 1/(2*L) * (xsigmak*xsigmak') - xpre * xpre' + Q;
%--- 2. Update----------------------------------
% Accelerometer measurements
a = imu_data(k, 5:7);
a = a - bias_a;
% We use the unit vector of acceleration as observation
ea = a' / norm(a);
% Estimate the square-root factor of the predicted error covariance matrix
S2 = chol(P)';
xpre_sigmak = S2*kesai + repmat(xpre, 1, 2*L);
zsigmak = zeros(3, 2*L);
for i = 1 : 2*L
q_pre = rod2quat(xpre_sigmak(:,i)');
zsigmak(:,i) = [2*(q_pre(2)*q_pre(4) - q_pre(1)*q_pre(3)); ...
2*(q_pre(3)*q_pre(4) + q_pre(1)*q_pre(2)); ...
q_pre(1)^2-q_pre(2)^2 - q_pre(3)^2+q_pre(4)^2];
end
zpre = sum(zsigmak, 2)/(2*L);
% Measurement noise R
R_internal = (noise_accel / norm(a))^2 * eye(3);
R_external = (1-gravity/norm(a))^2 * eye(3);
R = R_internal + R_external;
% residual, measurement covariance
Pzz = 1/(2*L)*(zsigmak*zsigmak') - zpre*zpre' + R;
Pxz = 1/(2*L)*(xpre_sigmak*zsigmak') - xpre*zpre';
y = ea - zpre;
% Update
K = Pxz/Pzz;
x = rod2quat((xpre + K*y)')';
P = P - K*Pzz*K';
% 3. Ending
x = x / norm(x); % Normalize the quaternion
P = (P + P') / 2; % Make sure that covariance matrix is symmetric
allX(k,:) = x'; % Save the result
end
测试结果:
估计方向曲线:
RMSE (deg) | EKF | CKF |
---|---|---|
yaw | 0.3953 | 0.4011 |
pitch | 0.3782 | 0.2349 |
roll | 0.4412 | 0.3438 |
每帧耗时:EKF: 0.024 ms, CKF: 0.15ms。
CKF的精度虽高,但是耗时较之EKF远甚。