Merge pull request mannyray#5 from mannyray/java_implementation

Java implementation

README.md

@@ -7,11 +7,12 @@ The `Matlab` and `C++` code are featured in the `matlab_implementation` and `c++
  - [Matlab documentation](http://kalmanfilter.org/matlab/index.html)
  - [C++ documentation](http://kalmanfilter.org/cpp/index.html)
  - [Python documentation](http://kalmanfilter.org/python/index.html)
+ - [Java documentation](http://kalmanfilter.org/java/index.html)
 
 
 ### Introduction
 
-This repository contains `Matlab`, `C++` and `Python` implementations of different Kalman filters. The insipiration to create this repository is rlabbe's github [repository](https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python) which is a great introduction to the Kalman filter in `python`. Go there first if you need a solid introduction the filter that will provide you with intuition behind its mechanics. 
+This repository contains `Matlab`, `C++`, `Java` and `Python` implementations of different Kalman filters. The insipiration to create this repository is rlabbe's github [repository](https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python) which is a great introduction to the Kalman filter in `python`. Go there first if you need a solid introduction the filter that will provide you with intuition behind its mechanics. 
 
 
 This repository aims to provide users a basic and ready to use arsenal to use in exploring filtering. I spent some time working with the Kalman Filter as part of my [thesis](https://uwspace.uwaterloo.ca/bitstream/handle/10012/14740/Zonov_Stanislav.pdf ) (see chapter 3) where I coded up continuous-discrete extended Kalman filter and discrete-discrete extended Kalman filter. After coding up the two filters, I decided to keep things interesting and added other filters as well (_UKF_,_Ensemble_,_Particle_). In order to test my implementations, I used the filters in various contexts as well as checked if the steady state covariances match as explained in [1].

generateDocumentation.sh

@@ -44,3 +44,18 @@ cp examples/*.png ../../documentation/python/
 rm -rf latex/*
 rmdir latex
 cd ../..
+
+pwd
+cd java_implementation
+rm -rf html/*
+rm -rf latex/*
+rmdir html
+rmdir latex
+pwd
+doxygen Doxyfile
+mv html ../documentation/java
+cp examples/*.png ../documentation/java/
+rm -rf latex/*
+rmdir latex
+cd ../..
+

java_implementation/.gitignore

@@ -0,0 +1 @@
+*.class

java_implementation/Function.java

@@ -0,0 +1,10 @@
+public class Function{
+
+        public Matrix next( Matrix x, double time){
+                return Matrix.zero(1,1);
+        }
+
+        public Matrix jacobian(Matrix x, double time){
+                return Matrix.zero(1,1);
+        }
+}

java_implementation/Gaussian.java

@@ -0,0 +1,71 @@
+import java.io.IOException;
+import java.util.*;
+import java.lang.Math;
+
+public class Gaussian{
+
+	public static Matrix [] computeGaussian( Matrix A, boolean shifts ){
+		int rowCount = A.rowCount();
+		int colCount = A.colCount();
+		if(rowCount != colCount){
+			throw new RuntimeException("computeGaussian expects a square matrix");
+		}
+
+		Matrix U = new Matrix(A);
+		Matrix L = Matrix.identity(rowCount,rowCount);
+		Matrix P = Matrix.identity(rowCount,rowCount);
+
+		for(int column=0; column<colCount; column++){
+			if(shifts){
+				double maxVal = Math.abs(U.get(column,column));
+				int maxIndex = column;
+
+				for(int row=column; row<rowCount; row++){
+					if(Math.abs(U.get(row,column)) > maxVal){
+						maxVal = Math.abs(U.get(row,column));
+						maxIndex = row;
+					}
+				}
+				if(maxIndex!=column){
+					U.swapRows(column,maxIndex,column,colCount);
+					L.swapRows(column,maxIndex,0,column);
+					P.swapRows(column,maxIndex,0,colCount);
+				}
+			}
+
+			for(int row=column+1; row<colCount; row++){
+				L.set(row,column,U.get(row,column)/U.get(column,column));
+				for(int column2=column; column2<rowCount; column2++){
+					U.set(row,column2,U.get(row,column2)-L.get(row,column)*U.get(column,column2));
+				}
+			}
+		}
+
+		if(shifts){
+			Matrix result [] = new Matrix[3];
+			result[0] = P;
+			result[1] = L;
+			result[2] = U;
+			return result;
+		}
+		else{
+			Matrix result [] = new Matrix[2];
+			result[0] = L;
+			result[1] = U;
+			return result;
+		}
+	}
+
+	public static Matrix[] computeGaussian( Matrix A ){
+		return computeGaussian(A,true);
+	}
+
+	public static Matrix solve(Matrix A, Matrix b){
+		Matrix arr [] = computeGaussian( A );
+		Matrix P = arr[0];
+		Matrix L = arr[1];
+		Matrix U = arr[2];
+
+		return TriangleSolve.upperTriangleSolve(U,TriangleSolve.lowerTriangleSolve(L,Matrix.multiply(P,b)));
+	}
+}

java_implementation/KalmanFilter.java

@@ -0,0 +1,88 @@
+import java.io.IOException;
+import java.util.*;
+
+public class KalmanFilter{
+
+	public static Matrix[] predictPhase( Function f, double time,  Matrix P_0_sqrt, Matrix x, Matrix Q_root){
+		int state_count = x.rowCount();
+		Matrix estimate = f.next(x,time);
+		Matrix jacobian = f.jacobian(x,time);
+
+		Matrix tmp = Matrix.zero(state_count, state_count*2);
+		tmp.setSubmatrix(0,0,state_count,state_count,Matrix.multiply(jacobian,P_0_sqrt));
+		tmp.setSubmatrix(0,state_count,state_count,state_count,Q_root);
+
+		Matrix QR_2[] = QR.QR(tmp.transpose());
+		Matrix Q = QR_2[0];
+		Matrix R = QR_2[1];
+
+		Matrix covariance_sqrt = R.transpose();
+		covariance_sqrt = covariance_sqrt.getSubmatrix(0,0,state_count,state_count);
+
+		Matrix result[] = new Matrix[2];
+		result[0] = estimate;
+		result[1] = covariance_sqrt;
+		return result;
+	}
+
+	public static Matrix[] updatePhase( Matrix R_root, Matrix P_root, Matrix C, Matrix estimate, Matrix measurement){
+		int measurement_count = C.rowCount();
+		int state_count = estimate.rowCount();
+
+		Matrix tmp = Matrix.zero(state_count+measurement_count,state_count+measurement_count);
+		tmp.setSubmatrix(0,0,measurement_count,measurement_count, R_root);
+		tmp.setSubmatrix(0,measurement_count,measurement_count,state_count,Matrix.multiply(C,P_root));
+		tmp.setSubmatrix(measurement_count,measurement_count,state_count,state_count,P_root);
+
+		Matrix QR_2[] = QR.QR(tmp.transpose());
+		Matrix Q = QR_2[0];
+		Matrix R = QR_2[1];
+		R = R.transpose();
+
+		Matrix X = R.getSubmatrix(0,0,measurement_count,measurement_count);
+		Matrix Y = R.getSubmatrix(measurement_count,0,state_count,measurement_count);
+		Matrix Z = R.getSubmatrix(measurement_count,measurement_count,state_count,state_count);
+
+		Matrix estimate_next = Matrix.plus(estimate,Matrix.multiply(Y,Gaussian.solve(X,Matrix.minus(measurement,Matrix.multiply(C,estimate)))));
+		Matrix covariance_sqrt = Z;
+
+		Matrix result[] = new Matrix[2];
+		result[0] = estimate_next;
+		result[1] = covariance_sqrt;
+		return result;
+	}
+
+	public static Matrix[][] ddekf( Function f, double dt_between_measurements, double start_time, int state_count, int sensor_count, int measurement_count, Matrix C, Matrix Q_root, Matrix R_root, Matrix P_0_root, Matrix x_0, Matrix[] measurements){
+		Matrix x_km1_p = x_0;
+		Matrix P_root_km1_p = P_0_root;
+
+		Matrix[] estimates = new Matrix[measurement_count+1];
+		Matrix[] covariances = new Matrix[measurement_count+1];
+
+		estimates[0] = x_km1_p;
+		covariances[0] = P_root_km1_p;
+
+		double current_time = start_time;
+
+		for(int k=0; k<measurement_count; k++){
+			Matrix resPredict[] = predictPhase(f,current_time,P_root_km1_p,x_km1_p,Q_root);
+			Matrix x_k_m = resPredict[0];
+			Matrix P_root_km = resPredict[1];
+
+			Matrix resUpdate[] = updatePhase(R_root,P_root_km,C,x_k_m,measurements[k]);
+
+			x_km1_p = resUpdate[0];
+			P_root_km1_p = resUpdate[1];
+
+			current_time = current_time + dt_between_measurements;
+
+			estimates[k+1] = x_km1_p;
+			covariances[k+1] = Matrix.multiply(P_root_km1_p,P_root_km1_p.transpose());
+		}
+
+		Matrix result [][] = new Matrix[2][];		
+		result[0] = estimates;
+		result[1] = covariances;
+		return result;
+	}
+}