我正在尝试用陀螺仪、加速度计和磁力计构建指南针。

我将 acc 值与磁力计值融合以获得方向(使用旋转矩阵)并且它工作得很好。

但现在我想添加陀螺仪来帮助补偿磁传感器不准确的情况。所以我想使用卡尔曼滤波器来融合这两个结果并得到一个很好的过滤结果(acc 和 mag 已经使用 lpf 进行了过滤)。

我的矩阵是:

 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 开始并以某种速率增加，而不管测量的一(罗盘)，它不会停止只是继续增加......

我不明白我做错了什么？

最佳答案

您看到的问题是，虽然陀螺仪的噪声很低，但它不是零均值。当您使用术语 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 小得多，因为偏差的变化不是很快(事实上，最好的模型可能是一阶高斯-马尔可夫过程，我不会在这里解释，除了在“有限内存过滤器”中抛出另一个有用的 Google 术语)。在极限情况下，您可以想象偏差的 Q 项为 0(将偏差建模为随机常数)，但这在 EKF 中的数值效果不佳，由于偏差漂移，严格来说并不正确。
• 同样，系统的初始 P_0 对于偏置项(其总可能范围记录在数据表中)比对于完全未知的航向/角速度要小得多。
• 在多轴系统中，偏差始终随轴变化(这是硬件的一个属性，与其定向方式无关)，但陀螺仪对“航向”等状态的影响正在旋转，因为捷联式 IMU。

看着 EKF“学习”像陀螺仪偏差这样的值对我来说比预测其他状态更神奇。

关于android - 卡尔曼滤波器 - 指南针和陀螺仪，我们在Stack Overflow上找到一个类似的问题： https://stackoverflow.com/questions/15275477/