Я пытаюсь построить компас с гироскопом, акселерометром и магнитометром.
Я объединяю значения 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 и увеличивается с некоторой скоростью, независимо от измеренного (компас), и он не остановится, просто продолжит увеличиваться...
Я не могу понять, что я делаю неправильно?
