android - 卡尔曼滤波器-指南针和陀螺仪

我正在尝试用陀螺仪，加速度计和测距仪构建指南针。

我将acc值与magnometer值融合在一起以获得方向(使用旋转矩阵)，并且效果很好。

但是现在我想添加陀螺仪，以在磁传感器不准确时进行补偿。因此，我想使用卡尔曼滤波器将这两个结果融合在一起，并得到一个很好的滤波结果(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}

然后，我观察两个结果(卡尔曼之一和指南针一个)。卡尔曼(Kalman)1从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“学习”陀螺仪偏差之类的值对我来说，比预测该州其他地区更具有魔力。