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

【问题标题】：Kalman Filter - Compass and Gyro卡尔曼滤波器 - 指南针和陀螺仪
【发布时间】：2013-03-07 15:45:14
【问题描述】：

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

我将 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}

然后我观察两个结果（卡尔曼结果和罗盘结果）。卡尔曼卡从 0 开始并以某种速度增加，无论测量的卡尔曼（罗盘）如何，它都不会停止，只是继续增加......

我不明白我做错了什么？

【问题讨论】：

你为什么要自己融合这些数据？平台提供的那个有什么问题？
如果我错了，请纠正我，但 android 只提供 acc+mag 融合
不，AFAIK 陀螺仪也被考虑在内。
好的，我会检查一下，但无论如何，出于学习目的，任何人都可以回答我的问题吗？
是的.. 你是对的.. android 只在本地融合了 mag 和 acc.. 不用多说，没有多少设备有陀螺仪。我发现你的帖子研究了在将低通或卡尔曼滤波器直接传递到罗盘之前将其融合到加速度计的选项。如果有上帝，我们希望他/她是堆栈溢出的开发人员。它是如此头脑麻木的问题。

标签： android kalman-filter

【解决方案1】：

您看到的问题是，虽然陀螺仪的噪声非常低，但它并不是零均值。当您使用术语e^2(gyro) 时，您正在实施一个过滤器，您声称z_gyro = true_gyro + v where 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 “学习”一个像陀螺仪偏差这样的值对我来说比预测其他状态更神奇。

【讨论】：