From eae5660fe3ccd18e7e114c97ca018e562b0830c0 Mon Sep 17 00:00:00 2001
From: Steven Lange <langestx@informatik.hu-berlin.de>
Date: Fri, 4 May 2018 08:51:52 +0200
Subject: [PATCH] Kalmanfilter implementation step 1 (1/2).
---
.../{environment.config => kalman.config} | 27 ++--
.../src/car/include/environment/Environment.h | 12 +-
.../car/include/environment/KalmanFilter.h | 43 ++++++
.../src/car/src/environment/Environment.cpp | 67 ---------
.../src/car/src/environment/KalmanFilter.cpp | 131 ++++++++++++++++++
5 files changed, 189 insertions(+), 91 deletions(-)
rename modules/CarConfig/{environment.config => kalman.config} (63%)
create mode 100644 modules/catkin_ws/src/car/include/environment/KalmanFilter.h
create mode 100644 modules/catkin_ws/src/car/src/environment/KalmanFilter.cpp
diff --git a/modules/CarConfig/environment.config b/modules/CarConfig/kalman.config
similarity index 63%
rename from modules/CarConfig/environment.config
rename to modules/CarConfig/kalman.config
index 9ee0d91d..42850675 100644
--- a/modules/CarConfig/environment.config
+++ b/modules/CarConfig/kalman.config
@@ -1,20 +1,21 @@
// 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
diff --git a/modules/catkin_ws/src/car/include/environment/Environment.h b/modules/catkin_ws/src/car/include/environment/Environment.h
index c869343c..555eecbd 100644
--- a/modules/catkin_ws/src/car/include/environment/Environment.h
+++ b/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
diff --git a/modules/catkin_ws/src/car/include/environment/KalmanFilter.h b/modules/catkin_ws/src/car/include/environment/KalmanFilter.h
new file mode 100644
index 00000000..e9bd1c53
--- /dev/null
+++ b/modules/catkin_ws/src/car/include/environment/KalmanFilter.h
@@ -0,0 +1,43 @@
+#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
diff --git a/modules/catkin_ws/src/car/src/environment/Environment.cpp b/modules/catkin_ws/src/car/src/environment/Environment.cpp
index 7fb8a84c..4e9547ea 100644
--- a/modules/catkin_ws/src/car/src/environment/Environment.cpp
+++ b/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");
diff --git a/modules/catkin_ws/src/car/src/environment/KalmanFilter.cpp b/modules/catkin_ws/src/car/src/environment/KalmanFilter.cpp
new file mode 100644
index 00000000..278e356c
--- /dev/null
+++ b/modules/catkin_ws/src/car/src/environment/KalmanFilter.cpp
@@ -0,0 +1,131 @@
+#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;
+}
+}
--
GitLab