documentation revamp and C++ predict fix

abicco · Jan 21, 2020 · 73d1fe0 · 73d1fe0
1 parent 27b0f80
commit 73d1fe0
Show file tree

Hide file tree

Showing 42 changed files with 4,634 additions and 671 deletions.
diff --git a/README.md b/README.md
diff --git a/assets/logistic1.png b/assets/logistic1.png
diff --git a/assets/logistic2.png b/assets/logistic2.png
diff --git a/c++_implementation/examples/parameterEstimation/analysis.py b/c++_implementation/examples/parameterEstimation/analysis.py
@@ -0,0 +1,42 @@
+import numpy as np
+import matplotlib.pyplot as plt
+from mpl_toolkits.axes_grid1 import make_axes_locatable
+
+results = [];
+for assumedProcessNoise in np.arange(0.05,0.25,0.01):
+    localResults = []
+    for assumedSensorNoise in [0.01]:#np.arange(0.1,0.6,0.01):
+        measurementFile = open("assumedP_"+"{0:.2f}".format(assumedProcessNoise)+
+            "assumedR"+"{0:.2f}".format(assumedSensorNoise)+"measurements.txt")
+        estimatesBeforeFile = open("assumedP_"+"{0:.2f}".format(assumedProcessNoise)+
+            "assumedR"+"{0:.2f}".format(assumedSensorNoise)+"estimatesBefore.txt")
+        measurements =  np.array([float(i) for i in measurementFile.readlines()])
+        #measurements = measurements[1:500]
+        estimates = np.array([float(i) for i in estimatesBeforeFile.readlines()])
+        #estimates = estimates[1:500]
+        localResults.append(sum(measurements - estimates)/len(measurements)) 
+        print sum(measurements - estimates)/len(measurements) 
+    results.insert(0,localResults)
+
+print results
+print measurements - estimates
+print sum(measurements - estimates)
+
+
+for i in results:
+    for j in i:
+        print round(j,4),
+    print
+
+fig,ax = plt.subplots()
+divider = make_axes_locatable(ax)
+cax = divider.append_axes('right', size='5%', pad=0.05)
+im = ax.imshow(results,extent=(0.4,0.6,0.4,0.6))
+fig.colorbar(im,cax=cax,orientation='vertical')
+plt.show()
+
+plt.plot(estimates)
+plt.show()
+
+
+
diff --git a/c++_implementation/examples/parameterEstimation/basicExampleNonlinear.cpp b/c++_implementation/examples/parameterEstimation/basicExampleNonlinear.cpp
@@ -0,0 +1,215 @@
+#include <iostream>
+#include <sstream>
+#include "../../include/Eigen.h"
+#include "../../include/KalmanFilter.h"
+#include "../../include/filterModel.h"
+#include "../../include/model.h"
+#include <fstream>
+#include <math.h>
+#include <iomanip>     
+#include<stdio.h>
+#include<stdlib.h>
+
+
+class processNoise: public discreteNoiseCovariance<Eigen::VectorXd,Eigen::MatrixXd>{
+	Eigen::MatrixXd Q;
+	Eigen::MatrixXd Q_root;
+	public:
+		processNoise(double v){
+			Eigen::MatrixXd tmp1(1,1);
+			tmp1<<v;
+			Q = tmp1;
+			Eigen::MatrixXd tmp2(1,1);
+			tmp2<<std::sqrt(v);
+			Q_root = tmp2;
+		}
+		Eigen::MatrixXd function(const Eigen::VectorXd &est, int t) override{
+			return Q;
+		}
+		Eigen::MatrixXd sqrt(const Eigen::VectorXd &est, int t) override{
+			return  Q_root; 
+		}
+};
+
+class sensorNoise: public discreteNoiseCovariance<Eigen::VectorXd,Eigen::MatrixXd>{
+	Eigen::MatrixXd R;
+	Eigen::MatrixXd R_root;
+	public:
+		sensorNoise(double v){
+			Eigen::MatrixXd tmp1(1,1);
+			tmp1<<v;
+			R = tmp1;
+			Eigen::MatrixXd tmp2(1,1);
+			tmp2<<std::sqrt(v);
+			R_root = tmp2;
+		}
+		Eigen::MatrixXd function(const Eigen::VectorXd &est, int t) override{
+			return R;
+		}
+		Eigen::MatrixXd sqrt(const Eigen::VectorXd &est, int t) override{
+			return R_root; 
+		}
+};
+
+class transitionJac: public jacobianDiscrete<Eigen::VectorXd,Eigen::MatrixXd>{
+	double rate;
+	double max_pop;
+	Eigen::VectorXd add;
+	public:
+		transitionJac(double rate, double max_pop):rate(rate),max_pop(max_pop){
+			Eigen::VectorXd tmp(1);
+			tmp<<1+rate;
+			add = tmp;
+		}
+		Eigen::MatrixXd function(const Eigen::VectorXd & val, int t){
+			return add - (2*rate/max_pop)*val;
+		}
+};
+
+class measurementJac: public jacobianDiscrete<Eigen::VectorXd,Eigen::MatrixXd>{
+
+	public:
+		Eigen::MatrixXd function(const Eigen::VectorXd & val, int t){
+			return Eigen::MatrixXd::Identity(1,1);
+		}
+};
+
+class stateModel: public discreteModel<Eigen::VectorXd>{
+	double rate;
+	double max_pop;
+	Eigen::VectorXd one;
+	public:
+		stateModel(double rate, double max_pop):rate(rate),max_pop(max_pop){
+			Eigen::VectorXd tmp(1);tmp<<1;
+			one = tmp;
+		}
+		Eigen::VectorXd function(const Eigen::VectorXd & val, const int time) const override{
+			return (val + rate*val*(one-val/max_pop));
+		}
+};
+
+
+class measurementModel: public discreteModel<Eigen::VectorXd>{
+	public:
+		measurementModel(){
+		}
+		Eigen::VectorXd function(const Eigen::VectorXd & val, const int time) const override{
+			return val;
+		}
+};
+
+using namespace std;
+
+int main(int argc, char**argv){
+
+	if(argc != 6){
+		cout<<"Incorrect arguments."<<endl; 
+		return -1;
+	}
+
+	string fileName;
+	double actualProcessNoise = 0;
+	double actualSensorNoise = 0;
+	double assumedProcessNoise = 0;
+	double assumedSensorNoise = 0;
+	string filePrefix = "";
+	try{
+		actualProcessNoise = strtod(argv[1],NULL);
+		actualSensorNoise = strtod(argv[2],NULL);
+		assumedProcessNoise = strtod(argv[3],NULL);
+		assumedSensorNoise = strtod(argv[4],NULL);
+		filePrefix = argv[5];
+	}catch(...){
+		cout<<"Incorrect arguments."<<endl; 
+	}
+
+	double rate = 0.01;
+	double max_pop = 100;
+
+	stateModel tm(rate,max_pop);
+	measurementModel mm;
+	processNoise pnActual(actualProcessNoise);
+	sensorNoise snActual(actualSensorNoise);
+	processNoise pnAssumed(assumedProcessNoise);
+	sensorNoise snAssumed(assumedSensorNoise);
+	transitionJac tj(rate,max_pop);
+
+	measurementJac mj;	
+	int stateCount = 1;
+	int sensorCount = 1;
+	discreteDiscreteFilterModel<Eigen::VectorXd,Eigen::MatrixXd>ddfmActual(&tm,&mm,&pnActual,&snActual,&tj,&mj,stateCount,sensorCount);
+	discreteDiscreteFilterModel<Eigen::VectorXd,Eigen::MatrixXd>ddfmAssumed(&tm,&mm,&pnAssumed,&snAssumed,&tj,&mj,stateCount,sensorCount);
+
+	int initialTime = 0;
+	Eigen::VectorXd initialState(1);
+	initialState<<(max_pop/2.0);
+	discreteDiscreteFilterSolver<Eigen::VectorXd,Eigen::MatrixXd> ddfs(&ddfmActual,initialTime,initialState);
+
+	int steps = 2000;
+	ddfs.setSeed(assumedProcessNoise*assumedSensorNoise*actualProcessNoise*actualSensorNoise);
+	ddfs.solve(steps);
+
+
+	Eigen::VectorXd * states = ddfs.getSolvedStates();
+	Eigen::VectorXd * measurements = ddfs.getSolvedMeasurements();
+
+
+	Eigen::VectorXd initialEstimate(1);
+	initialEstimate<<(max_pop/2.0);
+	Eigen::MatrixXd initialCovariance = Eigen::MatrixXd::Identity(1,1);
+
+	discreteDiscreteKalmanFilter<Eigen::VectorXd,Eigen::MatrixXd> filter(initialTime, initialEstimate, initialCovariance,&ddfmAssumed);
+
+
+	Eigen::VectorXd* estimatesBefore = new Eigen::VectorXd[steps];
+	Eigen::VectorXd* estimates = new Eigen::VectorXd[steps];
+	Eigen::MatrixXd* covariances = new Eigen::MatrixXd[steps];
+	for(int i = 0; i < steps; i++){
+		covariances[i] = filter.getCurrentCovariance();	
+		filter.predict(1);
+		estimatesBefore[i] = filter.getCurrentEstimate();
+		filter.update(measurements[i]);
+		estimates[i] = filter.getCurrentEstimate();
+	}
+
+	Eigen::IOFormat HeavyFmt(Eigen::FullPrecision);
+	std::ofstream outputfileEstimates;
+	stringstream ss1; ss1<<filePrefix<<"estimates.txt";
+	outputfileEstimates.open(ss1.str());
+	for(int i = 0; i < steps; i++){
+		outputfileEstimates<<estimates[i].format(HeavyFmt)<<std::endl;
+	}outputfileEstimates.close();
+
+	std::ofstream outputfileEstimatesBefore;
+	stringstream ss1_5; ss1_5<<filePrefix<<"estimatesBefore.txt";
+	outputfileEstimatesBefore.open(ss1_5.str());
+	for(int i = 0; i < steps; i++){
+		outputfileEstimatesBefore<<estimatesBefore[i].format(HeavyFmt)<<std::endl;
+	}outputfileEstimatesBefore.close();
+
+	std::ofstream outputfileMeasurements;
+	stringstream ss2; ss2<<filePrefix<<"measurements.txt";
+	outputfileMeasurements.open(ss2.str());
+	for(int i = 0; i < steps; i++){
+		outputfileMeasurements<<measurements[i].format(HeavyFmt)<<std::endl;
+	}outputfileMeasurements.close();
+
+	std::ofstream outputfileStates;
+	stringstream ss3; ss3<<filePrefix<<"states.txt";
+	outputfileStates.open(ss3.str());
+	for(int i = 0; i < steps; i++){
+		outputfileStates<<states[i].format(HeavyFmt)<<std::endl;
+	}outputfileStates.close();
+
+	std::ofstream outputfileCovariances;
+	stringstream ss4; ss4<<filePrefix<<"covariances.txt";
+	outputfileCovariances.open(ss4.str());
+	for(int i = 0; i < steps; i++){
+		outputfileCovariances<<covariances[i].format(HeavyFmt)<<std::endl;
+	}outputfileCovariances.close();
+
+	delete [] estimates;
+	delete [] states;
+	delete [] measurements;
+	delete [] covariances;
+}
diff --git a/c++_implementation/examples/parameterEstimation/compute.sh b/c++_implementation/examples/parameterEstimation/compute.sh
@@ -0,0 +1,19 @@
+#!/bin/bash
+
+echo "compiling"
+g++ basicExampleNonlinear.cpp -o script
+
+actualProcessNoise=0.1;
+actualSensorNoise=0.01;
+for assumedProcessNoise in $(seq 0.05 0.01 0.25); do
+	echo "ASSUMED PROCESS NOISE: " $assumedProcessNoise
+	#for assumedSensorNoise in $(seq 0.05 0.01 0.1); do
+		#echo "assumed sensor noise: " $assumedSensorNoise
+		fileSave="assumedP_"$assumedProcessNoise"assumedR"$actualSensorNoise	
+		./script $actualProcessNoise $actualSensorNoise $assumedProcessNoise $actualSensorNoise $fileSave
+	#done
+done
+
+
+
+
diff --git a/c++_implementation/examples/parameterEstimation/script b/c++_implementation/examples/parameterEstimation/script
diff --git a/c++_implementation/include/KalmanFilter.h b/c++_implementation/include/KalmanFilter.h
@@ -125,14 +125,17 @@ class discreteDiscreteKalmanFilter: public KalmanFilter<VECTOR,MATRIX,int>{
 			initialEstimate,initialCovariance,model){}
 
 		void predict(int timeUnitsForward){//TODO - have to run things time steps forward
-			VECTOR est = KF::getCurrentEstimate();
-			int time = KF::getCurrentTime();  
-			MATRIX jacob = KF::getTransitionJacobian(est,KF::getCurrentTime());
-			MATRIX covariance = KF::getCurrentCovariance();
-
-			KF::setCurrentEstimate(KF::transition(est,time));
-			KF::setCurrentCovariance(jacob*covariance*jacob.transpose() + KF::getProcessNoiseCovariance(est,time));
-			KF::setCurrentTime(time + 1);
+			for(int i = 0; i < timeUnitsForward; i++){
+				VECTOR est = KF::getCurrentEstimate();
+				int time = KF::getCurrentTime();  
+				MATRIX jacob = KF::getTransitionJacobian(est,KF::getCurrentTime());
+				MATRIX covariance = KF::getCurrentCovariance();
+
+				KF::setCurrentEstimate(KF::transition(est,time));
+
+				KF::setCurrentCovariance(jacob*covariance*jacob.transpose() + KF::getProcessNoiseCovariance(est,time));
+				KF::setCurrentTime(time + 1);
+			}
 		}
 
 		void update(VECTOR measurement){
@@ -144,6 +147,8 @@ class discreteDiscreteKalmanFilter: public KalmanFilter<VECTOR,MATRIX,int>{
 			MATRIX Sk = KF::getSensorNoiseCovariance(est,time) + measurementJacobian*covariance*measurementJacobian.transpose();
 			MATRIX KalmanGain = covariance*measurementJacobian.transpose()*Sk.inverse();
 			KF::setCurrentEstimate(est + KalmanGain*(measurement - KF::measure(est,time)));
+
+
 			KF::setCurrentCovariance((MATRIX::Identity(KF::getStateCount(),KF::getStateCount())-KalmanGain*measurementJacobian)*covariance);
 		}
 };
@@ -154,9 +159,6 @@ class discreteDiscreteKalmanFilterSquareRoot: public KalmanFilter<VECTOR,MATRIX,
 	using KF = KalmanFilter<VECTOR,MATRIX,int>;
 
 
-	MATRIX getCurrentCovariance(){
-		return currentCovarianceSQRT*currentCovarianceSQRT.transpose();
-	}	
 
 	private:
 		MATRIX currentCovarianceSQRT;
@@ -166,6 +168,11 @@ class discreteDiscreteKalmanFilterSquareRoot: public KalmanFilter<VECTOR,MATRIX,
 		}
 
 	public:
+
+	MATRIX getCurrentCovariance(){
+		return currentCovarianceSQRT*currentCovarianceSQRT.transpose();
+	}	
+
 		discreteDiscreteKalmanFilterSquareRoot(
 			int initialTime,
 			VECTOR initialEstimate,
@@ -178,22 +185,24 @@ class discreteDiscreteKalmanFilterSquareRoot: public KalmanFilter<VECTOR,MATRIX,
 			}
 
 		void predict(int timeUnitsForward){
-			int stateCount = KF::getStateCount();
-			int sensorCount = KF::getSensorCount();
-			VECTOR est = KF::getCurrentEstimate();
-			int time = KF::getCurrentTime();  
-			MATRIX jacob = KF::getTransitionJacobian(est,KF::getCurrentTime());
-			MATRIX covarianceSQRT = getCurrentCovarianceSQRT();
-
-			MATRIX tmp(stateCount,stateCount*2);//TODO: empty constructor
-			tmp.block(0,0,stateCount,stateCount) = jacob*covarianceSQRT;
-			tmp.block(0,stateCount,stateCount,stateCount) = KF::getProcessNoiseCovarianceSqrt(est,time);
-
-			MATRIX::lowerTriangulate(tmp);
-
-			KF::setCurrentEstimate(KF::transition(est,time));
-			currentCovarianceSQRT = tmp.block(0,0,stateCount,stateCount);
-			KF::setCurrentTime(time + 1);
+			for(int i = 0; i < timeUnitsForward; i++){
+				int stateCount = KF::getStateCount();
+				int sensorCount = KF::getSensorCount();
+				VECTOR est = KF::getCurrentEstimate();
+				int time = KF::getCurrentTime();  
+				MATRIX jacob = KF::getTransitionJacobian(est,KF::getCurrentTime());
+				MATRIX covarianceSQRT = getCurrentCovarianceSQRT();
+
+				MATRIX tmp(stateCount,stateCount*2);//TODO: empty constructor
+				tmp.block(0,0,stateCount,stateCount) = jacob*covarianceSQRT;
+				tmp.block(0,stateCount,stateCount,stateCount) = KF::getProcessNoiseCovarianceSqrt(est,time);
+
+				MATRIX::lowerTriangulate(tmp);
+
+				KF::setCurrentEstimate(KF::transition(est,time));
+				currentCovarianceSQRT = tmp.block(0,0,stateCount,stateCount);
+				KF::setCurrentTime(time + 1);
+			}
 		}
 
 		void update(VECTOR measurement){

diff --git a/matlab_implementation/.gitignore b/matlab_implementation/.gitignore
@@ -0,0 +1 @@
+Doc/*