卡尔曼滤波-罗盘和陀螺

Question

我想用陀螺仪，加速度计和磁力计来建造指南针。

我把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开始，并以某种速度增长，无论测量的罗盘(指南针)，它不会停止，只是不断增加。

我不明白我做错了什么？

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要小得多，因为偏差的演化速度并不快(事实上，最好的模型可能是一阶Gauss过程，我在这里不会解释这个过程，只是在“有限内存过滤器”中去掉另一个有用的谷歌术语)。在这个极限中，你可以想象偏差的Q项为0(将偏差建模为一个随机常数)，但在EKF中，这在数值上并不有效，而且由于偏差的漂移，严格来说也不是真的。
• 同样，系统的初始P_0对于偏置项(其总可能范围被记录在数据表中)要比完全未知的航向/角速度小得多。
• 在多轴系统中，偏置总是与轴(这是与其定向无关的硬件特性)，但是陀螺仪对像“航向”这样的状态的影响是由于带紧的IMU而旋转的。

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