Matlab 扩展卡尔曼滤波收敛到错误值，这就是野兽的本性吗？（图片）

我设计了一个四元数扩展卡尔曼滤波器，用于融合陀螺仪和加速度计数据。估计图的形状似乎很完美，但估计值似乎不断收敛到错误的解。这仅仅是因为我没有使用像线性卡尔曼滤波器这样的最优估计器，还是可能使用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

---

我不是卡尔曼滤波方面的专家，但我认为静态误差是陀螺/加速度测量的最佳结果。在我之前的实验室中，他们将惯性MEA与GPS融合以重新校准。

您描述的问题据说是EKF的主要缺点之一。它不能保证收敛到实际值。如果您想继续使用它：

• 尝试增加系统噪声

  Q

  和/或测量噪声

  R

  （可解释为“将非线性放入噪声中”）。这也使得线性KF在非线性问题上表现得更好
• 为了判断你做得有多好，在你的估计值周围画出2西格玛带，看看你的真实值是否在其中（否则你的噪声太小）

---

Unscented卡尔曼滤波器在处理非线性方面比EKF更具鲁棒性。实现的复杂性与EKF大致相同。

我刚刚尝试了您的算法，它运行得非常好。为了避免滤波器的输出跟随漂移积分角速率，需要减小测量噪声的方差值。我尝试过使用100而不是1000000的值。通过这种方式，您可以告诉过滤器更多地“信任”观察结果，因此，角度估计值不会发散

我还实现了一个脚本，它使用外部角度参考优化固定参数（过程噪声和测量噪声的方差）。这样就更容易约束它们的值

---

还需要注意的是，R和Q的值将取决于运动的强度，即“破坏”加速计估计值的线加速度量。为了提高估计的精度，我已经实现了一个强度检测器，它根据运动的强度为每个样本改变R和Q的值。如果您感兴趣，请通过：alberto.olivares（gmail.com）与我联系，我将很乐意将其发送给您（或任何其他感兴趣的人）

目前，我正在测试

EKF

和

互补滤波器的性能，以通过四元数形式的传感器融合（
gyro
，
acc
，
mag
）来估计姿态。最终的范围是得到不同的速度估计。现在我有一个非线性互补滤波器，它可以很好地估计姿态，然后减去重力，积分得到速度。它在短时间内运行良好

将此代码作为我的代码，并考虑到我的<代码> EKF 的测量输出是加速度计读数，其中惯性加速度是不可忽略的。因此，我的预测输出矩阵“
h
”是
z=T*acc
的结果，其中
T
是从传感器到本地帧的变换矩阵，
acc
是受重力影响的加速计读数。假设几乎没有偏航旋转，但只有显著的部分旋转（
一些人与我联系，他们有与第一篇文章中所述类似的问题。我想包括我的Github存储库，您可以在其中找到一个完整的库来使用MIMU估计方向

这些方法包括：加速度和磁场投影（这只是为了说明为什么需要融合）、常规卡尔曼滤波、扩展卡尔曼滤波、选通卡尔曼滤波和选通扩展卡尔曼滤波

你还可以找到一篇论文，解释我在会议上发表论文的过程和幻灯片

每件事都有完整的评论，所以我希望你能够理解每一步都做了些什么

如果你对此有任何疑问，请告诉我


干杯！
你能用图例标出你的图吗？卡尔曼滤波器的输出是蓝色的海军线，还是浅蓝色的？深蓝色是集成陀螺仪，浅蓝色/青色是EKF估计值，你可能想问一下。据我所知，绿线覆盖了红线，青色线在下面。所以两个卡尔曼有不同的端点。这是t他是对的？你有任何参考方位数据吗？线性（红色）卡尔曼滤波器是正确的解决方案，我试图将其与扩展KF（青色）匹配抱歉，我应该说，这是一个姿态估计，所以不需要GPS。好的，对于姿态估计，我们使用了不同的估计，基于本文：