我设计了一个四元数扩展卡尔曼滤波器，用于融合陀螺仪和加速度计数据。估计图的形状似乎很完美，但估计似乎不断收敛到错误的解决方案。这仅仅是因为我没有使用像线性卡尔曼滤波器这样的最佳估计器，还是应该可以使用 EKF 获得无偏估计？到目前为止，我已经使用了两种不同的实现，并且两次都遇到了同样的问题。
这是滤波器输出的图：
- 绿色：仅从加速度计估计的角度
- 蓝色：集成陀螺仪输出
- 红色：线性 KF 输出
- 青色：EKF输出，注意偏移
这是一次迭代的matlab代码：
function [ q, wb ] = EKF2( a,w,dt )
persistent x P;
% Tuning paramaters
Q = [0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0.01, 0, 0;
0, 0, 0, 0, 0, 0.01, 0;
0, 0, 0, 0, 0, 0, 0.01];
R = [1000000, 0, 0;
0, 1000000, 0;
0, 0, 1000000;];
if isempty(P)
P = eye(length(Q))*10000; %Large uncertainty of initial values
x = [1, 0, 0, 0, 0, 0, 0]';
end
q0 = x(1);
q1 = x(2);
q2 = x(3);
q3 = x(4);
wxb = x(5);
wyb = x(6);
wzb = x(7);
wx = w(1);
wy = w(2);
wz = w(3);
z(1) = a(1);
z(2) = a(2);
z(3) = a(3);
z=z';
% Populate F jacobian
F = [ 1, (dt/2)*(wx-wxb), (dt/2)*(wy-wyb), (dt/2)*(wz-wzb), -(dt/2)*q1, -(dt/2)*q2, -(dt/2)*q3;
-(dt/2)*(wx-wxb), 1, (dt/2)*(wz-wzb), -(dt/2)*(wy-wyb), (dt/2)*q0, (dt/2)*q3, -(dt/2)*q2;
-(dt/2)*(wy-wyb), -(dt/2)*(wz-wzb), 1, (dt/2)*(wx-wxb), -(dt/2)*q3, (dt/2)*q0, (dt/2)*q1;
-(dt/2)*(wz-wzb), (dt/2)*(wy-wyb), -(dt/2)*(wx-wxb), 1, (dt/2)*q2, -(dt/2)*q1, (dt/2)*q0;
0, 0, 0, 0, 1, 0, 0;
0, 0, 0, 0, 0, 1, 0;
0, 0, 0, 0, 0, 0, 1;];
%%%%%%%%% PREDICT %%%%%%%%%
%Predicted state estimate
% x = f(x,u)
x = [q0 - (dt/2) * (-q1*(wx-wxb) - q2*(wy-wyb) - q3*(wz-wzb));
q1 - (dt/2) * ( q0*(wx-wxb) + q3*(wy-wyb) - q2*(wx-wzb));
q2 - (dt/2) * (-q3*(wx-wxb) + q0*(wy-wyb) + q1*(wz-wzb));
q3 - (dt/2) * ( q2*(wx-wxb) - q1*(wy-wyb) + q0*(wz-wzb));
wxb;
wyb;
wzb;];
% Re-normalize Quaternion
qnorm = sqrt(x(1)^2 + x(2)^2 + x(3)^2 + x(4)^2);
x(1) = x(1)/qnorm;
x(2) = x(2)/qnorm;
x(3) = x(3)/qnorm;
x(4) = x(4)/qnorm;
q0 = x(1);
q1 = x(2);
q2 = x(3);
q3 = x(4);
% Predicted covariance estimate
P = F*P*F' + Q;
%%%%%%%%%% UPDATE %%%%%%%%%%%
% Normalize Acc and Mag measurements
acc_norm = sqrt(z(1)^2 + z(2)^2 + z(3)^2);
z(1) = z(1)/acc_norm;
z(2) = z(2)/acc_norm;
z(3) = z(3)/acc_norm;
h = [-2*(q1*q3 - q0*q2);
-2*(q2*q3 + q0*q1);
-q0^2 + q1^2 + q2^2 - q3^2;];
%Measurement residual
% y = z - h(x), where h(x) is the matrix that maps the state onto the measurement
y = z - h;
% The H matrix maps the measurement to the states
H = [ 2*q2, -2*q3, 2*q0, -2*q1, 0, 0, 0;
-2*q1, -2*q0, -2*q3, -2*q2, 0, 0, 0;
-2*q0, 2*q1, 2*q2, -2*q3, 0, 0, 0;];
% Measurement covariance update
S = H*P*H' + R;
% Calculate Kalman gain
K = P*H'/S;
% Corrected model prediction
x = x + K*y; % Output state vector
% Re-normalize Quaternion
qnorm = sqrt(x(1)^2 + x(2)^2 + x(3)^2 + x(4)^2);
x(1) = x(1)/qnorm;
x(2) = x(2)/qnorm;
x(3) = x(3)/qnorm;
x(4) = x(4)/qnorm;
% Update state covariance with new knowledge
I = eye(length(P));
P = (I - K*H)*P; % Output state covariance
q = [x(1), x(2), x(3), x(4)];
wb = [x(5), x(6), x(7)];
end