Android 卡尔曼滤波-罗盘和陀螺

我正在尝试用陀螺仪、加速度计和磁强计来制作罗盘

我正在融合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}

然后我观察两个结果（卡尔曼和罗盘）。kalman 1从0开始，以一定的速率增加，与测量值（指南针）无关，它不会停止，只是继续增加

---

我不明白我做错了什么？

你看到的问题是，虽然陀螺仪的噪声非常低，但它不是零均值。当你使用术语

e^2（陀螺仪）

时，你正在实现一个过滤器，你声称

z_gyro=true_gyro+v

其中

v~N（0，e^2）

事实更像

v~N（偏置，e^2）

即使偏置也有几个术语（主要是静态开启偏置加上由温度漂移引起的偏置偏移）。因此，您正在积分偏置并不断旋转

如果校准了该偏差（静止时仅测量陀螺仪的输出），则可以调用

update（imu-bias）

而不是

update（imu）

。您可能必须增加

e^2（陀螺仪）

以说明偏差的变化，但不能像您试图说明所有偏差那样多（未补偿的偏移量将变成与磁强计和陀螺仪的

R

项成比例的固定航向位移）

最好的方法是将偏差添加到状态向量中。你会得到类似于

Hk={{1,0,0}，{0,1,1}}

，这意味着你预测的陀螺测量值是真实速率加上偏差项。Kalman滤波器的神奇之处在于，尽管你说过你的测量值只是两项的总和，但它们在几个关键方面是不同的：

• 在

  F

  中，航向与实际转向率相关（通过

  dt

  ），因此每次更新

  P

  时，状态协方差

  P

  演变为与航向和转向率相关的非对角项
• 类似地，在

  H

  中，您描述了偏差和陀螺仪速率之间的关系，表达了“要么我转得更快，要么我有更多偏差”的想法，因此过滤器根据噪声协方差更新状态以平衡这两种可能性
• 在

  Q

  中，必须将转弯率过程噪声设置得相当高，以说明您正在测量的任何意外运动。但是偏差的

  Q

  要小得多，因为偏差的发展不是很快（事实上，最好的模型可能是一阶高斯-马尔可夫过程，我在这里不会解释，除了在“有限内存过滤器”中抛出另一个有用的谷歌术语）。在极限情况下，您可以想象偏差的

  Q

  项为0（将偏差建模为随机常数），但这在EKF中数值上不起作用，并且由于偏差漂移，这不是严格正确的
• 类似地，系统的初始

  P_0

  偏差项（其总可能范围记录在数据表中）比完全未知的航向/角速度小得多
• 在多轴系统中，偏置总是随轴变化（这是硬件的特性，与定向无关），但陀螺对“航向”等状态的影响因捷联惯性测量单元而旋转

---

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

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