我正在尝试使用陀螺仪,加速度计和万用表来构建罗盘。
我将acc值与测量值融合以获得方向(使用旋转矩阵),并且它运行良好。
但现在我想添加陀螺仪,以便在磁传感器不准确时进行补偿。因此,我想使用kalman过滤器来融合两个结果并得到一个很好的过滤结果(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开始并且以某种速率增加而不管测量的那个(罗盘)它不会停止只是继续增加......
我无法理解我做错了什么?
答案 0 :(得分:6)
您所看到的问题是,虽然陀螺仪的噪音非常低,但并非零均值。当您使用字词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
以考虑偏差的变化,但不会像您尝试考虑所有偏差一样多(未补偿的偏移将变为与{{1成比例的固定航向位移)磁力计和陀螺仪的术语。
最好的方法是将偏差添加到状态向量中。你得到类似Hk = {{1,0,0},{0,1,1}}
的东西,这意味着你预测的陀螺仪测量值是真实的速度加上你的偏差项。卡尔曼滤波器的神奇之处在于,尽管您已经说过您的测量只是两个项的总和,但它们在几个关键方面有所不同:
F
中,标题与实际转弯率(dt
)相关,因此状态协方差P
演变了与标题和转弯率相关的非对角线条款你更新P
。H
你已经描述了偏差和陀螺仪速率之间的关系,表达了“要么我转得更快,要么我有更多偏见”的想法,所以过滤器更新状态以平衡这两种可能性基于噪声协方差。Q
中,转弯率过程噪音必须设置得相当高,以应对您测量的任何意外运动。但偏差的Q
要小得多,因为偏差不是很快发展的(实际上,最好的模型可能是一阶高斯 - 马尔可夫过程,我不会在这里解释,除了抛弃“有限内存过滤器”中另一个有用的Google术语。在极限情况下,你可以想象偏差的Q
项为0(建模偏差为随机常数),但在EKF中数字效果不佳,并且不是'由于偏差漂移,严格来说是真的。P_0
对于偏差项(数据表中记录的总可能范围)要小于完全未知的航向/角速度。看着EKF“学习”像陀螺仪偏见这样的值对我来说比对其他州的预测更神奇。