卡尔曼滤波器 - 罗盘和陀螺仪

Question

我正在尝试使用陀螺仪、加速度计和磁力计来构建罗盘。

我正在将加速度计值与磁力计值进行融合，以获取方向（使用旋转矩阵），并且效果相当不错。

但现在我想添加陀螺仪来帮助补偿磁传感器不准确的情况。因此，我想使用卡尔曼滤波器融合两个结果，得到一个良好的滤波结果（已经使用低通滤波器对加速度计和磁力计进行了滤波）。

我的矩阵如下：

 state(Xk) => {Compass Heading, Rate from the gyro in that axis}.
 transition(Fk) => {{1,dt},{0,1}}
 measurement(Zk) => {Compass Heading, Rate from the gyro in that axis}
 Hk => {{1,0},{0,1}}
 Qk = > {0,0},{0,0}
 Rk => {e^2(compass),0},{0,e^2(gyro)}

以下是我的卡尔曼滤波器实现代码：

public class KalmanFilter {

private Matrix x,F,Q,P,H,K,R;
private Matrix y,s;

public KalmanFilter(){
}

public void setInitialState(Matrix _x, Matrix _p){
    this.x = _x;
    this.P = _p;
}

public void update(Matrix z){
    try {
        y = MatrixMath.subtract(z, MatrixMath.multiply(H, x));
        s = MatrixMath.add(MatrixMath.multiply(MatrixMath.multiply(H, P), 
                        MatrixMath.transpose(H)), R);
        K = MatrixMath.multiply(MatrixMath.multiply(P, H), MatrixMath.inverse(s));
        x = MatrixMath.add(x, MatrixMath.multiply(K, y));
        P = MatrixMath.subtract(P, 
                        MatrixMath.multiply(MatrixMath.multiply(K, H), P));
    } catch (IllegalDimensionException e) {
        e.printStackTrace();
    } catch (NoSquareException e) {
        e.printStackTrace();
    }
    predict();
}

private void predict(){
    try {
        x = MatrixMath.multiply(F, x);
        P = MatrixMath.add(Q, MatrixMath.multiply(MatrixMath.multiply(F, P), 
                        MatrixMath.transpose(F)));
    } catch (IllegalDimensionException e) {
        e.printStackTrace();
    }
}

public Matrix getStateMatirx(){
    return x;
}

public Matrix getCovarianceMatrix(){
    return P;
}

public void setMeasurementMatrix(Matrix h){
    this.H = h;
}

public void setProcessNoiseMatrix(Matrix q){
    this.Q = q;
}

public void setMeasurementNoiseMatrix(Matrix r){
    this.R = r;
}

public void setTransformationMatrix(Matrix f){
    this.F = f;
}
}

首先，给出了这些起始值：

 Xk => {0,0}
 Pk => {1000,0},{0,1000}

我会观察两个结果（卡尔曼和指南针）。卡尔曼的结果从0开始，并以某种速率增加，与被测量的结果（指南针）无关，它不会停止，只会持续增加...

我不明白我做错了什么？

- user1396033

---

你为什么要自己合并这些数据？平台提供的有什么问题吗？ - Ali

请纠正我如果我错了，但 Android 只提供了加速度计和磁力计融合的功能。 - user1396033

不，据我所知陀螺仪也是被考虑在内的。 - Ali

好的，我会检查一下，但是无论如何，出于学习目的，任何人都可以回答我的问题吗？ - user1396033

是的，你说得对。安卓只能本地融合磁力计和加速度计，而且很少有设备带陀螺仪。我在研究将低通滤波器或卡尔曼滤波器直接传递给指南针，然后再将其与加速度计融合的选项时发现了你的帖子。如果有神明的话，让我们希望他/她是 Stack Overflow 上的开发者...这是一个令人头痛的问题。 - erik

Answer 1

你看到的问题是，陀螺仪虽然噪声很小，但不是零均值。当你使用术语e^2(gyro)时，你正在实现一个滤波器，其中你声称z_gyro = true_gyro + v，其中v ~ N(0, e^2)。事实上，更像是v ~ N(bias, e^2)，即使偏差也有一些项（主要是静态开机偏差加上温漂引起的偏差变化）。因此，你正在积分偏差并不断旋转。

如果你校准掉这个偏差（只需测量静止时陀螺仪的输出），那么你可以调用update(imu - bias)而不仅仅是update(imu)。你可能需要增加e^2(gyro)来考虑偏差的变化，但不需要考虑所有偏差（未补偿的偏移将成为与磁力计和陀螺仪的R项成比例的固定航向位移）。

最好的方法是将偏差添加到状态向量中。你会得到类似于Hk = {{1,0,0},{0,1,1}}的东西，这意味着你的预测陀螺仪测量值是真实速率加上偏差项。卡尔曼滤波器的奇妙之处在于，即使你已经说过你的测量只是两个术语的总和，它们在几个关键方面是不同的：

• 在F中，标题与实际转向速率（由dt确定）有关，因此每次更新P时，状态协方差P会演变成涉及标题和转向速率的非对角线项。
• 同样，在H中，您已经描述了偏差和陀螺仪速率之间的关系，表达了“要么我转得更快，要么我的偏差更大”的想法，因此滤波器根据噪声协方差平衡这两种可能性来更新状态。
• 在Q中，转向速率过程噪声必须设置得相当高，以解释您正在测量的任何意外运动。但是，偏差的Q要小得多，因为偏差不会发生很快的演变（事实上，最好的模型可能是一阶高斯马尔可夫过程，我将不在此解释，只提供另一个有用的谷歌术语“有限内存滤波器”）。在极限情况下，您可以将偏差的Q项设为0（将偏差建模为随机常数），但这在EKF中的数值计算中效果不佳，并且由于偏差漂移并不严格正确。
• 同样，系统的初始P_0对于偏差项要小得多（其总可能范围在数据表中有记录），而对于完全未知的标题/角速度则要大得多。
• 在多轴系统中，偏差始终与轴一起移动（这是与设备定向无关的硬件属性），但由于捆绑式IMU的影响，陀螺仪对于“标题”等状态的影响正在旋转。

看着EKF“学习”诸如陀螺仪偏差之类的值对我来说甚至比预测其余状态更神奇。

- Ben Jackson