adding kalman filter

contains basic 1-D implementation of Kalman filter concepts

Author: siddarth09

Kalman_filter/gaussian.hpp

@@ -0,0 +1,12 @@
+#include <iostream>
+#include <math.h>
+
+using namespace std;
+
+float gauss(float,float,float);
+
+float gauss(float mu,float sigma2,float x)
+{
+    float prob=(1.0/2.0*M_PI*sigma2)*exp(-0.5*(pow((x-mu),2)/sigma2));
+    return prob;
+}

Kalman_filter/main.cpp

@@ -0,0 +1,26 @@
+#include "gaussian.hpp"
+#include "measurement_update.hpp"
+#include "motion_update.hpp"
+
+int main()
+{
+    double measurements[5] = { 5, 6, 7, 9, 10 };
+    double measurement_sig = 4;
+    
+    //Motions and motion variance
+    double motion[5] = { 1, 1, 2, 1, 1 };
+    double motion_sig = 2;
+    
+    //Initial state
+    double mu = 50;
+    double sig = 28;
+
+    for(int i=0;i<sizeof(measurements)/sizeof(measurements[0]);i++)
+    {
+        tie(mu,sig)=parameter_update(mu,measurements[i],sig,measurement_sig);
+        printf("update:  [%f, %f]\n", mu, sig);
+        tie(mu, sig) = state_update(mu,  motion[i],sig, motion_sig);
+        printf("predict: [%f, %f]\n", mu, sig);
+
+    }
+}

Kalman_filter/main.exe

@@ -0,0 +1,12 @@
+#include <iostream>
+#include <math.h>
+#include <tuple>
+
+double newmu,newvar;
+tuple<double,double> parameter_update(double mu1,double mu2,double sigma2a, double sigma2b)
+{
+     newmu=(sigma2b*mu1+sigma2a*mu2)/(sigma2b+sigma2a);
+     newvar=1.0/(1.0/sigma2a+1.0/sigma2b);
+
+    return make_tuple(newmu,newvar);
+}

Kalman_filter/measurement_update.exe

@@ -0,0 +1,12 @@
+#include <iostream>
+#include <math.h>
+#include <tuple>
+
+double mu,var;
+tuple<double,double> state_update(double mu1,double mu2,double sigma2a, double sigma2b)
+{
+     mu=mu1+mu2;
+     var=sigma2a+sigma2b;
+
+    return make_tuple(mu,var);
+}

Kalman_filter/measurement_update.hpp

@@ -0,0 +1,74 @@
+#include <iostream>
+#include <math.h>
+#include <tuple>
+#include "Core" // Eigen Library
+#include "LU"   // Eigen Library
+
+using namespace std;
+using namespace Eigen;
+
+float measurements[3] = { 1, 2, 3 };
+
+tuple<MatrixXf, MatrixXf> kalman_filter(MatrixXf x, MatrixXf P, MatrixXf u, MatrixXf F, MatrixXf H, MatrixXf R, MatrixXf I)
+{
+    for (int n = 0; n < sizeof(measurements) / sizeof(measurements[0]); n++) {
+        //****** TODO: Kalman-filter function********//
+        
+        // Measurement Update
+        // Code the Measurement Update
+        // Initialize and Compute Z, y, S, K, x, and P
+        MatrixXf Z(1,1);
+        Z<<measurements[n];
+        
+        MatrixXf y(1,1);
+        y<<Z-H*x;
+        
+        MatrixXf S(1,1);
+        S<<H*P*H.transpose()+R;
+        
+        MatrixXf K(2,1);
+        K<<P*H.transpose()*S.inverse();
+        
+        x<<x+K*y;
+        
+        P << (I - (K * H)) * P;
+
+        // Prediction
+        x << (F * x) + u;
+        P << F * P * F.transpose();
+        
+    }
+
+    return make_tuple(x, P);
+}
+
+int main()
+{
+
+    MatrixXf x(2, 1);// Initial state (location and velocity) 
+    x << 0,
+    	 0; 
+    MatrixXf P(2, 2);//Initial Uncertainty
+    P << 100, 0, 
+    	 0, 100; 
+    MatrixXf u(2, 1);// External Motion
+    u << 0,
+    	 0; 
+    MatrixXf F(2, 2);//Next State Function
+    F << 1, 1,
+    	 0, 1; 
+    MatrixXf H(1, 2);//Measurement Function
+    H << 1,
+    	 0; 
+    MatrixXf R(1, 1); //Measurement Uncertainty
+    R << 1;
+    MatrixXf I(2, 2);// Identity Matrix
+    I << 1, 0,
+    	 0, 1; 
+
+    tie(x, P) = kalman_filter(x, P, u, F, H, R, I);
+    cout << "x= " << x << endl;
+    cout << "P= " << P << endl;
+
+    return 0;
+}