5

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

我将 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 开始并以某种速度增加，无论测量的一（罗盘）如何，它都不会停止，只是继续增加......

我不明白我做错了什么？

4

1 回答 1

6

您看到的问题是，虽然陀螺仪的噪声非常低，但它不是零均值。当您使用您的术语时，您正在实施一个过滤器，您e^2(gyro)声称 事实更像是即使偏置也有一些术语（主要是静态开启偏置加上由温度漂移引起的偏置偏移）。结果，您正在整合偏见并不断旋转。z_gyro = true_gyro + vv ~ 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，状态协方差都会演变出与航向和转弯率相关的非对角项。PP
  • 同样，H您已经描述了偏差和陀螺仪速率之间的关系，它表达了“要么我转得更快，要么我有更多偏差”的想法，因此过滤器更新状态以根据噪声协方差平衡这两种可能性。
  • 在Q中，必须将转弯速率过程噪声设置得相当高，以说明您正在测量的任何意外运动。但是Qfor 偏差要小得多，因为偏差不会很快演变（实际上，最好的模型可能是一阶高斯-马尔可夫过程，我不会在这里解释，除了扔掉另一个有用的谷歌术语“有限的内存过滤器”）。在极限情况下，您可以Q将偏差项想象为 0（将偏差建模为随机常数），但这在 EKF 中的数值上不能很好地工作，并且由于偏差漂移而不是严格正确的。
  • 同样，系统的初始P_0偏差项（其总可能范围记录在数据表中）比完全未知的航向/角速度要小得多。
  • 在多轴系统中，偏差总是随轴移动（这是硬件的一个属性，与它的方向无关）但是陀螺仪对像“航向”这样的状态的影响由于捷联而被旋转惯性测量单元。

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

于 2014-11-16T21:27:01.417 回答