package co.edu.unal.ing.accmodels.data_processing; import co.edu.unal.ing.accmodels.controller.VectorController; public class Kalman { private static final float q = 0.01f; private static final float r = 0.1f; public static final int TYPE_1D = 0; public static final int TYPE_3D = 1; private Filter filters[][]; public Kalman(){ filters = new Filter[3][2]; for(int i=0;i<filters.length;i++){ filters[i][TYPE_1D] = initialize1DFilter(); filters[i][TYPE_3D] = initialize3DFilter(); } } private static Filter initialize3DFilter(){ float dt = 1f; float A[][] = { {1, dt, dt*dt/2}, {0, 1, dt}, {0, 0, 1}}; float B[][] = VectorController.identity(3); float H[][] = VectorController.identity(3); float currentStateEstimate[][] = { {0}, {0}, {5}}; float currentProbEstimate[][] = VectorController.identity(3); float Q[][] = { {(float) (Math.pow(dt,5)/20f),(float) (Math.pow(dt,4)/8f),(float) (Math.pow(dt,3)/6f)}, {(float) (Math.pow(dt,4)/ 8f),(float) (Math.pow(dt,3)/3f),(float) (Math.pow(dt,2)/2f)}, {(float) (Math.pow(dt,3)/ 6f),(float) (Math.pow(dt,2)/2f), dt}}; Q = VectorController.scalar(q, Q); float R[][] = VectorController.scalar(r, VectorController.identity(3)); return new Filter(A, B, H, currentStateEstimate, currentProbEstimate, Q, R); } private static Filter initialize1DFilter(){ float A[][] = new float[1][1]; A[0][0] = 1f; float B[][] = new float[1][1]; B[0][0] = 0f; float H[][] = new float[1][1]; H[0][0] = 1f; float currentStateEstimate[][] = new float[1][1]; currentStateEstimate[0][0] = 0f; float currentProbEstimate[][] = new float[1][1]; currentProbEstimate[0][0] = 1f; float Q[][] = new float[1][1]; Q[0][0] = q; float R[][] = new float[1][1]; R[0][0] = r; return new Filter(A, B, H, currentStateEstimate, currentProbEstimate, Q, R); } public float[] filter(float[] input, int filterToUse){ //Process the input float ret[] = new float[3]; if(filterToUse == TYPE_3D){//Use 3D filter for(int i=0;i<3;i++){ ret[i] = filters[i][TYPE_3D].getCurrentState()[2][0]; float controlVector[][] = { {0}, {0}, {0}}; float measurementVector[][] = { {0}, {0}, {input[i]}}; filters[i][TYPE_3D].step(controlVector, measurementVector); } }else if(filterToUse == TYPE_1D){//Use 1D filters for(int i=0;i<3;i++){ ret[i] = filters[i][TYPE_1D].getCurrentState()[0][0]; float controlVector[][] = new float[1][1]; controlVector[0][0] = 0; float measurementVector[][] = new float[1][1]; measurementVector[0][0] = input[i]; filters[i][TYPE_1D].step(controlVector, measurementVector); } } return ret; } public static class Filter{ private float A[][]; //State transition matrix. private float B[][]; //Control matrix. private float H[][]; //Observation matrix. private float currentStateEstimate[][]; //Initial state estimate. private float currentProbEstimate[][]; //Initial covariance estimate. private float Q[][]; //Estimated error in process. private float R[][]; //Estimated error in measurements. public Filter(float A[][], float B[][], float H[][], float currentStateEstimate[][], float currentProbEstimate[][], float Q[][], float R[][]){ this.A = A; this.B = B; this.H = H; this.currentStateEstimate = currentStateEstimate; this.currentProbEstimate = currentProbEstimate; this.Q = Q; this.R = R; } public float[][] getCurrentState(){ return currentStateEstimate; } public void step(float controlVector[][], float measurementVector[][]){ //---------------------------Prediction step----------------------------- //predicted_state_estimate = A * current_state_estimate + B * control_vector float predictedStateEstimate[][] = VectorController.addMatrixes( VectorController.multiplyMatrixes(A, currentStateEstimate), VectorController.multiplyMatrixes(B, controlVector)); //predicted_prob_estimate = (A * current_prob_estimate) * transpose(A) + Q float predictedProbEstimate[][] = VectorController.addMatrixes( VectorController.multiplyMatrixes( VectorController.multiplyMatrixes(A, currentProbEstimate), VectorController.transponse(A)), Q); //--------------------------Observation step----------------------------- //innovation = measurement_vector - H*predicted_state_estimate float innovation[][] = VectorController.addMatrixes(measurementVector, VectorController.scalar(-1, VectorController.multiplyMatrixes(H, predictedStateEstimate))); //innovation_covariance = (H*predicted_prob_estimate)*transpose(H) + R float innovationCovariance[][] = VectorController.addMatrixes( VectorController.multiplyMatrixes( VectorController.multiplyMatrixes(H, predictedProbEstimate), VectorController.transponse(H)), R); //-----------------------------Update step------------------------------- //kalman_gain = predicted_prob_estimate * transpose(H) * inv(innovation_covariance) float kalmanGain[][] = VectorController.multiplyMatrixes( VectorController.multiplyMatrixes(predictedProbEstimate, VectorController.transponse(H)), VectorController.inverse(innovationCovariance)); //current_state_estimate = predicted_state_estimate + kalman_gain * innovation currentStateEstimate = VectorController.addMatrixes(predictedStateEstimate, VectorController.multiplyMatrixes(kalmanGain, innovation)); // We need the size of the matrix so we can make an identity matrix. int size = currentProbEstimate.length; //current_prob_estimate = (identity(size)-kalman_gain*self)*predicted_prob_estimate currentProbEstimate = VectorController.multiplyMatrixes( VectorController.addMatrixes( VectorController.identity(size), VectorController.scalar(-1, VectorController.multiplyMatrixes(kalmanGain, H))), predictedProbEstimate); } } }