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

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偏见的偏差要小得多,因为偏差并没有迅速发展(事实上,最好的模型可能是一阶高斯 - 马尔可夫过程,我不会在这里解释,除了抛出另一个有用的谷歌术语"有限的内存过滤器").在极限情况下,你可以想象Q偏差的项为0(建模偏差为随机常数),但在EKF中不能很好地数值,并且由于偏差漂移而不是严格正确.
• 同样,P_0系统的初始值对于偏置项(数据表中记录的总可能范围)要小于完全未知的航向/角速度.
• 在多轴系统中,偏置始终与轴一致(它是与其定向无关的硬件属性)但是陀螺仪对"前进"等状态的影响因为带式下降而被旋转IMU.

看着EKF"学习"像陀螺仪偏差这样的值对我来说比对其他州的预测更加神奇.