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

Question

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

我正在将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开始，并以一定的速度增加，无论测量到的（指南针），它不会停止只是继续增加...

我不明白我在做什么错？

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要小得多，因为偏差不是非常迅速地演变（事实上，最好的模型可能是一阶高斯 - 马尔可夫过程，除了抛出另一个有用的Google术语，我不会在这里解释在“有限内存过滤器”中）。在极限情况下，您可以想象Q项的偏差为0（模型偏差为随机常数），但在EKF中数值上不能很好地工作，并且由于偏差漂移而不完全正确。
• 类似地，系统的初始P_0对于偏置项（数据表中记录的总可能范围）要比完全未知的航向/角速度小得多。
• 在多轴系统中，偏差总是与轴线一致（这是硬件的一个属性，与它如何定向无关），但陀螺仪对像“航向”这样的状态的影响正在旋转，因为捷径IMU。

看着EKF“学习”一个像陀螺仪偏差这样的数值，对我来说比对其他州的预测更加神奇。