Kalmanfilter implementation step 1 (1/2).

modules/CarConfig/environment.config → modules/CarConfig/kalman.config

 // Numbers on name representing indicies
 // current car state
-x1: 0
-x2: 0
+x 2 1
+0
+0
 // covariance matrix
-p11: 10
-p22: 10
+P 2 2
+10 0
+0 10
 // dynamic model
-f11: 1
-f12: 0.001
-f21: 0
-f22: 1
 // process error
-q11: 1
-q22: 1
+Q 2 2
+1 0
+0 1
 // measure vector
-h1: 1
-h2: 0
+H 2 1
+1
+0
 // measurement error
-r: 5
+R 1 1
+5

modules/catkin_ws/src/car/include/environment/Environment.h

@@ -26,7 +26,6 @@ public:
 private:
     ros::NodeHandle nh_;
     std::string name_;
-    void readConfigFile();
 
     float distance;
     float relativeSpeed;
@@ -38,16 +37,7 @@ private:
     void ussDataCallback(const ussDataMsg::ConstPtr & inMsg);
     void camDataCallback(const camDataMsg::ConstPtr & inMsg);
 
-    //Notation like in Kalamanfilter <Wikipedia>
-    vector<double> x {2};          // current state (distance, velocity)^T
-    matrix<double> P {2, 2};       // covariance matrix
-    matrix<double> F {2, 2};       // dynamic model
-    matrix<double> Q {2, 2};       // process error
-    vector<double> H {2};          // mesure vector (refers to distance)
-    vector<double> R {1};          // measurement error
-    
-    void predict();
-    void update(vector<double> mesVec);
+
 };
 }
 #endif

modules/catkin_ws/src/car/include/environment/KalmanFilter.h

+#ifndef KALMANFILTER_H
+#define KALMANFILTER_H
+
+#include <boost/numeric/ublas/vector.hpp>
+#include <boost/numeric/ublas/matrix.hpp>
+#include <vector>
+#include <fstream>
+
+using namespace boost::numeric::ublas;
+namespace car
+{
+class KalmanFilter
+{
+public:
+    KalmanFilter(vector<double> x, matrix<double> P, matrix<double> F, matrix<double> Q, vector<double> H, vector<double> R, double *deltaT);
+
+    KalmanFilter(int dimension, matrix<double> F, double *deltaT, std::string configFilePath);
+
+    ~KalmanFilter();
+    
+    vector<double> nextKalmanIteration(vector <double> mesVec);
+
+private:
+    //Notation like in Kalamanfilter <Wikipedia>
+    vector<double> x;           // current state (distance, velocity)^T
+    matrix<double> P;           // covariance matrix
+    matrix<double> F;           // dynamic model
+    matrix<double> Q;           // process error
+    vector<double> H;           // mesure vector (refers to distance)
+    vector<double> R;           // measurement error
+
+    double *dt;                 // delta t
+    
+    void readConfigFile(std::string configFilePath);
+
+    std::vector<std::string> getNextFileLine(std::ifstream file);
+
+    void predict();
+
+    void update(vector<double> mesVec);
+};
+}
+#endif

modules/catkin_ws/src/car/src/environment/Environment.cpp

@@ -8,10 +8,6 @@
 #include "car/environmentDataMsg.h"
 #include "car/ussDataMsg.h"
 
-#include <fstream>
-#include <vector>
-#include <iterator>
-
 PLUGINLIB_EXPORT_CLASS(car::Environment, nodelet::Nodelet);
 
 namespace car
@@ -34,69 +30,6 @@ Environment::Environment() :
   
 Environment::~Environment() {}
  
-void Environment::readConfigFile()
-{
-    // open config file
-    std::string userHome = getenv("HOME");
-    std::ifstream configFile;
-    configFile.open(userHome + "/CarConfig/environment.config", std::ifstream::in);
-    if (!configFile.is_open()) { throw FileNotFound(); }
-
-    // desired parameters
-    
-    // read file
-    std::string contentLine;
-    while (!configFile.eof())
-    {
-        std::getline(configFile, contentLine);
-        
-        // split this line
-        std::istringstream iss(contentLine);
-        std::vector<std::string> words {std::istream_iterator<std::string>(iss), {} };
-
-        if ( words.size() > 1 ) // this line contains parameter[at (0)] and value [at (1)]
-        {
-            if (words.at(0) == "x1:") {
-                x(0) = std::stod(words.at(1));
-            } else if (words.at(0) == "x2:"){
-                x(1) = std::stof(words.at(1));
-            } else if (words.at(0) == "p11:"){
-                P(0, 0) = std::stof(words.at(1));
-            } else if (words.at(0) == "p12:"){
-                P(0,1) = std::stof(words.at(1));
-            } else if (words.at(0) == "p21:"){
-                P(1,0) = std::stof(words.at(1));
-            } else if (words.at(0) == "p22:"){
-                P(1,1) = std::stof(words.at(1));
-            } else if (words.at(0) == "f11:"){
-                F(0,0) = std::stof(words.at(1));
-            } else if (words.at(0) == "f12:"){
-                F(0,1) = std::stof(words.at(1));
-            } else if (words.at(0) == "f21:"){
-                F(1,0) = std::stof(words.at(1));
-            } else if (words.at(0) == "f22:"){
-                F(1,1) = std::stof(words.at(1));
-            } else if (words.at(0) == "q11:"){
-                Q(0,0) = std::stof(words.at(1));
-            } else if (words.at(0) == "q12:"){
-                Q(0,1) = std::stof(words.at(1));
-            } else if (words.at(0) == "q21:"){
-                Q(1,0) = std::stof(words.at(1));
-            } else if (words.at(0) == "q22:"){
-                Q(1,1) = std::stof(words.at(1));
-            } else if (words.at(0) == "h1:"){
-                H(0) = std::stof(words.at(1));
-            } else if (words.at(0) == "h2:"){
-                H(1) = std::stof(words.at(1));
-            } else if (words.at(0) == "r:"){
-                R(0) = std::stof(words.at(1));
-            }       
-        }
-    }
-    configFile.close();
-    return;
-}
-
 void Environment::onInit()
 {
     NODELET_INFO("Environment::onInit -- START");

modules/catkin_ws/src/car/src/environment/KalmanFilter.cpp

+#include "environment/KalmanFilter.h"
+#include <iterator>
+
+namespace car
+{
+KalmanFilter::KalmanFilter(vector<double> x, matrix<double> P, matrix<double> F, matrix<double> Q, vector<double> H, vector<double> R, double *deltaT)
+    : x(x)
+    , P(P)
+    , F(F)
+    , Q(Q)
+    , H(H)
+    , R(R)
+    , dt(deltaT)
+    {}
+
+KalmanFilter::KalmanFilter(int dimension, matrix<double> F, double *deltaT, std::string configFilePath)
+    : x(dimension)
+    , P(dimension, dimension)
+    , F(F)
+    , Q(dimension, dimension)
+    , H(dimension)
+    , R(dimension)
+    , dt(deltaT)
+    {
+        readConfigFile(configFilePath);
+    }
+
+KalmanFilter::~KalmanFilter() {}
+
+KalmanFilter::getNextFileLine(std::ifstream file)
+{
+    // read a line
+    std::string contentLine;
+    std::getline(file, contentLine);
+        
+    // split this line
+    std::istringstream iss(contentLine);
+    std::vector<std::string> words {std::istream_iterator<std::string>(iss), {} };
+    return words;
+}
+
+KalmanFilter::readConfigFile(std::string configFilePath)
+{
+    // open config file
+    std::string userHome = getenv("HOME");
+    std::ifstream configFile;
+    configFile.open(configFileLocation, std::ifstream::in);
+    if (!configFile.is_open()) { throw FileNotFound(); }
+
+    // read file
+    std::vector<std::string> words;
+    while (!configFile.eof())
+    {
+        words = getNextFileLine(configFile);
+        if ( words.size() == 3 ) // this line contains parameter[at (0)] and value [at (1)]
+        {
+            if ( words.at(0) == "//") { continue; }
+            int rows = std::stoi(words.at(1));
+            int cols = std::stoi(words.at(2));
+            int i = 0;
+            int j = 0;
+            if (words.at(0) == "x") {
+                while ( i < rows )
+                {
+                    words = getNextFileLine(configFile);
+                    x(i) = std::stod(words.at(0));
+                }
+            } else if (words.at(0) == "P"){
+                while ( i < rows )
+                {
+                    words = getNextFileLine(configFile);
+                    while ( j < cols )
+                    {
+                        P(i, j) = std::stod(words.at(j));
+                    }
+                }
+            } else if (words.at(0) == "Q"){
+                while ( i < rows )
+                {
+                    words = getNextFileLine(configFile);
+                    while ( j < cols )
+                    {
+                        Q(i, j) = std::stod(words.at(j));
+                    }
+                }
+            } else if (words.at(0) == "H"){
+                while ( i < rows )
+                {
+                    words = getNextFileLine(configFile);
+                    H(i) = std::stod(words.at(0));
+                }
+            } else if (words.at(0) == "R"){
+                while ( i < rows )
+                {
+                    words = getNextFileLine(configFile);
+                    x(i) = std::stod(words.at(j));
+                }
+            }       
+        }
+    }
+    configFile.close();
+    return;
+}
+
+KalmanFilter::predict(){
+    // x = F*x
+    // P = (F * P * F.T) + Q
+}
+
+KalmanFilter::update(vector<double> mesVec){
+    // Innovation
+    // y = mesVec.T, - H * x
+
+    // Residualkovarianz
+    // S = H * P * H.T + R
+
+    // Kalman - Gain
+    // K = P * H.T * S**(-1)
+    
+    // update the predecited values
+    // x = x + K * y
+    // P = P - K * S * K.T
+}
+
+KalmanFilter::nextKalmanIteration(vector <double> mesVec)
+{
+    update(mesVec);
+    predict();
+    return x;
+}
+}