我正在尝试使用陀螺仪、加速度计和磁力计来构建罗盘。
我正在将加速度计值与磁力计值进行融合,以获取方向(使用旋转矩阵),并且效果相当不错。
但现在我想添加陀螺仪来帮助补偿磁传感器不准确的情况。因此,我想使用卡尔曼滤波器融合两个结果,得到一个良好的滤波结果(已经使用低通滤波器对加速度计和磁力计进行了滤波)。
我的矩阵如下:
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开始,并以某种速率增加,与被测量的结果(指南针)无关,它不会停止,只会持续增加...
我不明白我做错了什么?