Я внедрил 3D-фильтр Калмана в Java, чтобы отфильтровать ускорение и найти скорость и положение, у меня есть ускорение в качестве данных датчика, но когда я применяю фильтрацию, результат не желателен, что, как предполагается, может быть, есть некоторая ошибка иЯ не могу понять, есть один класс MYKalmanFilter3D, и второй класс Matrix
, вот код класса MYKalmanFilter3D ..
public class MyKalmanFilter3D {
Matrix X; //State Space
Matrix P; //error coveriance
double T; //Delta T time
Matrix F; //Transition Matrix
double Q; //Proces Noise Matrix
double R; //Measurment Noise or variance in sensor
double Y; //Residual
Matrix K; //Kalman Gain
double Bu; //Model Control input
Matrix H; //Measurement funtion
//Cunstructer initializing the state space
public MyKalmanFilter3D(double accelration, double t, double r) { //recieving sensor initial value, time and noise in sensor
//State Space 3 x 1
double[][] x = new double[][]{{0.}, {0.}, {accelration}};
this.X = new Matrix(x);
//error coveriance 3 x 3
double[][] p = new double[][]{{10., 0., 0}, {0., 10, 0.}, {0., 0., 10.}};
this.P = new Matrix(p);
//Delta T time
this.T = t;
//State Transition Matrix 3 x 3
double[][] f = new double[][]{{1., T, 0.5 * (T * T)}, {0., 1., T}, {0., 0., 1.}};
this.F = new Matrix(f);
//Proces Noise Matrix
this.Q = 0;
//Measurment Noise or variance in sensor
this.R = r;
//Residual
this.Y = 0;
//Kalman Gain
double[][] k = new double[][]{{1.}, {1.}, {1.}};
this.K = new Matrix(k);
//Model Control input
this.Bu = 0;
//Measurement Funtion
double[][] h = new double[][]{{0., 0., 1.}};
this.H = new Matrix(h);
}
//getter for accelration in state space
double estimatedAccelration() {
return X.elementAt(2, 0);
}
//getter for velocity in state space
double velocity() {
return X.elementAt(1, 0);
}
//getter for position in state space
double position() {
return X.elementAt(0, 0);
}
//Predict
// X' = X*F + B*u
// P' = F*P*Ft + Q
public void predict() {
X = F.times(X);
P = F.times(P).times(F.transpose());
}
//Update
// Y = Z - H*X'
// K = P*H't / (H*P*H't + R)
// X = X' + K*Y
// p = (1 - K*H)*P'
public void update(double Z) { //here is Z measurement value from sensor which is to be filter
Y = Z - H.times(X).elementAt(0, 0);
K = P.times(H.transpose()).dividedByNumber((H.times(P.times(H.transpose())).elementAt(0, 0) + R));
X = X.plus(K.multiplyByNumber(Y));
P = (K.numberSubtractedByMatrix(1).times(H)).times(P);
}
}
и вот код класса Matrix ...
import java.io.OutputStreamWriter;
import java.io.PrintWriter;
import java.io.UnsupportedEncodingException;
import java.util.Locale;
import android.util.Log;
final public class Matrix {
private final int M; // number of rows
private final int N; // number of columns
private final double[][] data; // M-by-N array
// create M-by-N matrix of 0's
public Matrix(int M, int N) {
this.M = M;
this.N = N;
data = new double[M][N];
}
// create matrix based on 2d array
public Matrix(double[][] data) {
M = data.length;
N = data[0].length;
this.data = new double[M][N];
for (int i = 0; i < M; i++)
for (int j = 0; j < N; j++)
this.data[i][j] = data[i][j];
}
// copy constructor
private Matrix(Matrix A) { this(A.data); }
// create and return a random M-by-N matrix with values between 0 and 1
public static Matrix random(int M, int N) {
Matrix A = new Matrix(M, N);
for (int i = 0; i < M; i++)
for (int j = 0; j < N; j++)
A.data[i][j] = Math.random();
return A;
}
// create and return the N-by-N identity matrix
public static Matrix identity(int N) {
Matrix I = new Matrix(N, N);
for (int i = 0; i < N; i++)
I.data[i][i] = 1;
return I;
}
// swap rows i and j
private void swap(int i, int j) {
double[] temp = data[i];
data[i] = data[j];
data[j] = temp;
}
// create and return the transpose of the invoking matrix
public Matrix transpose() {
Matrix A = new Matrix(N, M);
for (int i = 0; i < M; i++)
for (int j = 0; j < N; j++)
A.data[j][i] = this.data[i][j];
return A;
}
// return A = A / number
public Matrix dividedByNumber(double num) {
Matrix A = this;
Matrix C = new Matrix(M, N);
for (int i = 0; i < M; i++)
for (int j = 0; j < N; j++)
C.data[i][j] = A.data[i][j] / num;
return C;
}
// return A = number - A
public Matrix numberSubtractedByMatrix(double num) {
Matrix A = this;
Matrix C = new Matrix(M, N);
for (int i = 0; i < M; i++)
for (int j = 0; j < N; j++)
C.data[i][j] = num - A.data[i][j];
return C;
}
// return A = A x number
public Matrix multiplyByNumber(double num) {
Matrix A = this;
Matrix C = new Matrix(M, N);
for (int i = 0; i < M; i++)
for (int j = 0; j < N; j++)
C.data[i][j] = A.data[i][j] * num;
return C;
}
// return C = A + B
public Matrix plus(Matrix B) {
Matrix A = this;
if (B.M != A.M || B.N != A.N) throw new RuntimeException("Illegal matrix dimensions.");
Matrix C = new Matrix(M, N);
for (int i = 0; i < M; i++)
for (int j = 0; j < N; j++)
C.data[i][j] = A.data[i][j] + B.data[i][j];
return C;
}
// return element at m x n
public double elementAt(int m, int n) {
Matrix A = this;
return A.data[m][n];
}
// return element at m x n of given Matrix
public double elementAt(int m, int n, Matrix M) {
return M.data[m][n];
}
// return C = A - B
public Matrix minus(Matrix B) {
Matrix A = this;
if (B.M != A.M || B.N != A.N) throw new RuntimeException("Illegal matrix dimensions.");
Matrix C = new Matrix(M, N);
for (int i = 0; i < M; i++)
for (int j = 0; j < N; j++)
C.data[i][j] = A.data[i][j] - B.data[i][j];
return C;
}
// does A = B exactly?
public boolean eq(Matrix B) {
Matrix A = this;
if (B.M != A.M || B.N != A.N) throw new RuntimeException("Illegal matrix dimensions.");
for (int i = 0; i < M; i++)
for (int j = 0; j < N; j++)
if (A.data[i][j] != B.data[i][j]) return false;
return true;
}
// return C = A * B
public Matrix times(Matrix B) {
Matrix A = this;
if (A.N != B.M) throw new RuntimeException("Illegal matrix dimensions.");
Matrix C = new Matrix(A.M, B.N);
for (int i = 0; i < C.M; i++)
for (int j = 0; j < C.N; j++)
for (int k = 0; k < A.N; k++)
C.data[i][j] += (A.data[i][k] * B.data[k][j]);
return C;
}
// return x = A^-1 b, assuming A is square and has full rank
public Matrix solve(Matrix rhs) {
if (M != N || rhs.M != N || rhs.N != 1)
throw new RuntimeException("Illegal matrix dimensions.");
// create copies of the data
Matrix A = new Matrix(this);
Matrix b = new Matrix(rhs);
// Gaussian elimination with partial pivoting
for (int i = 0; i < N; i++) {
// find pivot row and swap
int max = i;
for (int j = i + 1; j < N; j++)
if (Math.abs(A.data[j][i]) > Math.abs(A.data[max][i]))
max = j;
A.swap(i, max);
b.swap(i, max);
// singular
if (A.data[i][i] == 0.0) throw new RuntimeException("Matrix is singular.");
// pivot within b
for (int j = i + 1; j < N; j++)
b.data[j][0] -= b.data[i][0] * A.data[j][i] / A.data[i][i];
// pivot within A
for (int j = i + 1; j < N; j++) {
double m = A.data[j][i] / A.data[i][i];
for (int k = i+1; k < N; k++) {
A.data[j][k] -= A.data[i][k] * m;
}
A.data[j][i] = 0.0;
}
}
// back substitution
Matrix x = new Matrix(N, 1);
for (int j = N - 1; j >= 0; j--) {
double t = 0.0;
for (int k = j + 1; k < N; k++)
t += A.data[j][k] * x.data[k][0];
x.data[j][0] = (b.data[j][0] - t) / A.data[j][j];
}
return x;
}
// print matrix to standard output
public void show() {
for (int i = 0; i < M; i++) {
for (int j = 0; j < N; j++) {
System.out.print(data[i][j]);
System.out.print(" ");
}
//Log.d("MATRIX: ", String.valueOf();
System.out.print("\n");
//Log.d("","\n");
}
}
}