Skip to content

Commit 043a1d1

Browse files
AleksandarHaberAleksandarHaber
authored
Initial commit
1 parent bf67895 commit 043a1d1

6 files changed

+819
-0
lines changed

‎KalmanFilter.cpp

+223
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,223 @@
1+
// Implementation file of the Kalman filter class
2+
3+
#include <iostream>
4+
#include<tuple>
5+
#include<string>
6+
#include<fstream>
7+
#include<vector>
8+
#include<Eigen/Dense>
9+
#include "KalmanFilter.h"
10+
11+
using namespace Eigen;
12+
using namespace std;
13+
14+
// edit this later
15+
KalmanFilter::KalmanFilter()
16+
{
17+
}
18+
// Overloaded constructor
19+
// - takes A,B,C, Q, R, P0, x0 matrices/vectors and assigns them to private variables
20+
//
21+
// - the input argument "maxMeasurements" is used to initialize zero matrices
22+
// "estimatesAposteriori","estimatesApriori", "covarianceAposteriori", "covarianceApriori"
23+
// "gainMatrices", and "errors" that are used to store appropriate quantities during the propagation
24+
// of the Kalman filter equations
25+
//
26+
// - the private variable "k" is set to zero. This variable is used to track the current iteration
27+
// of the Kalman filter.
28+
KalmanFilter::KalmanFilter(MatrixXd Ainput, MatrixXd Binput, MatrixXd Cinput,
29+
MatrixXd Qinput, MatrixXd Rinput, MatrixXd P0input,
30+
MatrixXd x0input, unsigned int maxSimulationSamples)
31+
{
32+
k=0;
33+
// assign the private variables
34+
A=Ainput; B=Binput; C=Cinput; Q=Qinput;
35+
R=Rinput; P0=P0input; x0=x0input;
36+
// extract the appropriate dimensions
37+
n = A.rows(); m = B.cols(); r = C.rows();
38+
39+
// this matrix is used to store a posteriori estimates column wise
40+
estimatesAposteriori.resize(n, maxSimulationSamples);
41+
estimatesAposteriori.setZero();
42+
estimatesAposteriori.col(0)=x0;
43+
44+
// this matrix is used to store a priori estimates column wise
45+
estimatesApriori.resize(n, maxSimulationSamples);
46+
estimatesApriori.setZero();
47+
48+
// this matrix is used to store the a posteriori covariance matrices next to each other
49+
covarianceAposteriori.resize(n,n*maxSimulationSamples);
50+
covarianceAposteriori.setZero();
51+
covarianceAposteriori(all,seq(0,n-1))=P0;
52+
53+
// this matrix is used to store the a priori covariance matrices next to each other
54+
covarianceApriori.resize(n,n*maxSimulationSamples);
55+
covarianceApriori.setZero();
56+
57+
// this matrix is used to store the Kalman gain matrices next to each other
58+
gainMatrices.resize(n,r*maxSimulationSamples);
59+
gainMatrices.setZero();
60+
61+
// this matrix is used to store the errors (innovations) column-wise
62+
errors.resize(r,maxSimulationSamples);
63+
errors.setZero();
64+
}
65+
66+
// this member function predicts the estimate on the basis of the external input
67+
// it computes the a priori estimate
68+
// it computes the a priori covariance matrix
69+
void KalmanFilter::predictEstimate(MatrixXd externalInput)
70+
{
71+
// keep in mind that initially, k=0
72+
estimatesApriori.col(k)=A*estimatesAposteriori.col(k)+B*externalInput;
73+
covarianceApriori(all,seq(k*n,(k+1)*n-1))=A*covarianceAposteriori(all,seq(k*n,(k+1)*n-1))*(A.transpose())+Q;
74+
// increment the time step
75+
k++;
76+
77+
}
78+
79+
// this member function updates the estimate on the basis of the measurement
80+
// it computes the Kalman filter gain matrix
81+
// it computes the a posteriori estimate
82+
// it computes the a posteriori covariance matrix
83+
84+
void KalmanFilter::updateEstimate(MatrixXd measurement)
85+
{
86+
// initially, the value of k will be 1, once this function is called
87+
// this is because predictEstimate() is called before this function
88+
// and predict estimate increments the value of k
89+
90+
// this matrix is used to compute the Kalman gain
91+
MatrixXd Sk;
92+
Sk.resize(r,r);
93+
Sk=R+C*covarianceApriori(all,seq((k-1)*n,k*n-1))*(C.transpose());
94+
Sk=Sk.inverse();
95+
// gain matrices
96+
gainMatrices(all,seq((k-1)*r,k*r-1))=covarianceApriori(all,seq((k-1)*n,k*n-1))*(C.transpose())*Sk;
97+
// compute the error - innovation
98+
errors.col(k-1)=measurement-C*estimatesApriori.col(k-1);
99+
// compute the a posteriori estimate, remember that for k=0, the corresponding column is x0 - initial guess
100+
estimatesAposteriori.col(k)=estimatesApriori.col(k-1)+gainMatrices(all,seq((k-1)*r,k*r-1))*errors.col(k-1);
101+
102+
MatrixXd In;
103+
In= MatrixXd::Identity(n,n);
104+
MatrixXd IminusKC;
105+
IminusKC.resize(n,n);
106+
IminusKC=In-gainMatrices(all,seq((k-1)*r,k*r-1))*C; // I-KC
107+
108+
// update the a posteriori covariance matrix
109+
covarianceAposteriori(all,seq(k*n,(k+1)*n-1))
110+
=IminusKC*covarianceApriori(all,seq((k-1)*n,k*n-1))*(IminusKC.transpose())
111+
+gainMatrices(all,seq((k-1)*r,k*r-1))*R*(gainMatrices(all,seq((k-1)*r,k*r-1)).transpose());
112+
}
113+
114+
// this member function is used to load the measurement data from the external CSV file
115+
// the values are stored in the output matrix
116+
// MatrixXd is an Eigen typdef for Matrix<double, Dynamic, Dynamic>
117+
118+
MatrixXd KalmanFilter::openData(string fileToOpen)
119+
{
120+
121+
// the inspiration for creating this function was drawn from here (I did NOT copy and paste the code)
122+
// https://stackoverflow.com/questions/34247057/how-to-read-csv-file-and-assign-to-eigen-matrix
123+
// NOTE THAT THIS FUNCTION IS CALLED BY THE FUNCTION: SimulateSystem::openFromFile(std::string Afile, std::string Bfile, std::string Cfile, std::string x0File, std::string inputSequenceFile)
124+
125+
// the input is the file: "fileToOpen.csv":
126+
// a,b,c
127+
// d,e,f
128+
// This function converts input file data into the Eigen matrix format
129+
130+
131+
132+
// the matrix entries are stored in this variable row-wise. For example if we have the matrix:
133+
// M=[a b c
134+
// d e f]
135+
// the entries are stored as matrixEntries=[a,b,c,d,e,f], that is the variable "matrixEntries" is a row vector
136+
// later on, this vector is mapped into the Eigen matrix format
137+
vector<double> matrixEntries;
138+
139+
// in this object we store the data from the matrix
140+
ifstream matrixDataFile(fileToOpen);
141+
142+
// this variable is used to store the row of the matrix that contains commas
143+
string matrixRowString;
144+
145+
// this variable is used to store the matrix entry;
146+
string matrixEntry;
147+
148+
// this variable is used to track the number of rows
149+
int matrixRowNumber = 0;
150+
151+
152+
while (getline(matrixDataFile, matrixRowString)) // here we read a row by row of matrixDataFile and store every line into the string variable matrixRowString
153+
{
154+
stringstream matrixRowStringStream(matrixRowString); //convert matrixRowString that is a string to a stream variable.
155+
156+
while (getline(matrixRowStringStream, matrixEntry,',')) // here we read pieces of the stream matrixRowStringStream until every comma, and store the resulting character into the matrixEntry
157+
{
158+
matrixEntries.push_back(stod(matrixEntry)); //here we convert the string to double and fill in the row vector storing all the matrix entries
159+
}
160+
matrixRowNumber++; //update the column numbers
161+
}
162+
163+
// here we convert the vector variable into the matrix and return the resulting object,
164+
// note that matrixEntries.data() is the pointer to the first memory location at which the entries of the vector matrixEntries are stored;
165+
return Map<Matrix<double, Dynamic, Dynamic, RowMajor>> (matrixEntries.data(),
166+
matrixRowNumber, matrixEntries.size() / matrixRowNumber);
167+
168+
}
169+
170+
171+
// this member function saves the stored date in the corresponding CSV files
172+
173+
void KalmanFilter::saveData(string estimatesAposterioriFile, string estimatesAprioriFile,
174+
string covarianceAposterioriFile, string covarianceAprioriFile,
175+
string gainMatricesFile, string errorsFile) const
176+
{
177+
const static IOFormat CSVFormat(FullPrecision, DontAlignCols, ", ", "\n");
178+
179+
ofstream file1(estimatesAposterioriFile);
180+
if (file1.is_open())
181+
{
182+
file1 << estimatesAposteriori.format(CSVFormat);
183+
184+
file1.close();
185+
}
186+
187+
ofstream file2(estimatesAprioriFile);
188+
if (file2.is_open())
189+
{
190+
file2 << estimatesApriori.format(CSVFormat);
191+
file2.close();
192+
}
193+
194+
ofstream file3(covarianceAposterioriFile);
195+
if (file3.is_open())
196+
{
197+
file3 << covarianceAposteriori.format(CSVFormat);
198+
file3.close();
199+
}
200+
201+
ofstream file4(covarianceAprioriFile);
202+
if (file4.is_open())
203+
{
204+
file4 << covarianceApriori.format(CSVFormat);
205+
file4.close();
206+
}
207+
208+
ofstream file5(gainMatricesFile);
209+
if (file5.is_open())
210+
{
211+
file5 << gainMatrices.format(CSVFormat);
212+
file5.close();
213+
}
214+
215+
ofstream file6(errorsFile);
216+
if (file6.is_open())
217+
{
218+
file6 << errors.format(CSVFormat);
219+
file6.close();
220+
}
221+
222+
223+
}

‎KalmanFilter.h

+93
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,93 @@
1+
// header file for the Kalman filter class
2+
// Auhtor: Aleksandar Haber
3+
#ifndef KALMANFILTER_H
4+
#define KALMANFILTER_H
5+
6+
#include<string>
7+
#include<Eigen/Dense>
8+
using namespace Eigen;
9+
using namespace std;
10+
class KalmanFilter{
11+
12+
public:
13+
// Default constructor - edit this later
14+
KalmanFilter();
15+
16+
// Overloaded constructor
17+
// - takes A,B,C, Q, R, P0, x0 matrices/vectors and assigns them to private variables
18+
//
19+
// - the input argument "maxMeasurements" is used to initialize zero matrices
20+
// "estimatesAposteriori","estimatesApriori", "covarianceAposteriori", "covarianceApriori"
21+
// "gainMatrices", and "errors" that are used to store appropriate quantities during the propagation
22+
// of the Kalman filter equations
23+
//
24+
// - the private variable "k" is set to zero. This variable is used to track the current iteration
25+
// of the Kalman filter.
26+
KalmanFilter(MatrixXd A, MatrixXd B, MatrixXd C,
27+
MatrixXd Q, MatrixXd R, MatrixXd P0,
28+
MatrixXd x0, unsigned int maxSimulationSamples);
29+
30+
// this member function updates the estimate on the basis of the measurement
31+
// it computes the Kalman filter gain matrix
32+
// it computes the a posteriori estimate
33+
// it computes the a posteriori covariance matrix
34+
void updateEstimate(MatrixXd measurement);
35+
36+
// this member function predicts the estimate on the basis of the external input
37+
// it computes the a priori estimate
38+
// it computes the a priori covariance matrix
39+
void predictEstimate(MatrixXd externalInput);
40+
41+
// this member function is used to load the measurement data from the external CSV file
42+
// the values are stored in the output matrix
43+
// MatrixXd is an Eigen typdef for Matrix<double, Dynamic, Dynamic>
44+
static MatrixXd openData(string fileToOpen);
45+
46+
// this member function saves the stored date in the corresponding CSV files
47+
void saveData(string estimatesAposterioriFile, string estimatesAprioriFile,
48+
string covarianceAposterioriFile, string covarianceAprioriFile,
49+
string gainMatricesFile, string errorsFile) const;
50+
51+
private:
52+
53+
// this variable is used to track the current time step k of the estimator
54+
// after every measurement arrives, this variables is incremented for +1
55+
unsigned int k;
56+
57+
// m - input dimension, n- state dimension, r-output dimension
58+
unsigned int m,n,r;
59+
60+
// MatrixXd is an Eigen typdef for Matrix<double, Dynamic, Dynamic>
61+
MatrixXd A,B,C,Q,R,P0; // A,B,C,Q, R, and P0 matrices
62+
MatrixXd x0; // initial state
63+
64+
// this matrix is used to store the a posteriori estimates xk^{+} starting from the initial estimate
65+
// note: the estimates are stored column wise in this matrix, starting from
66+
// x0^{+}=x0 - where x0 is an initial guess of the estimate
67+
MatrixXd estimatesAposteriori;
68+
69+
// this matrix is used to store the a apriori estimates xk^{-} starting from x1^{-}
70+
// note: the estimates are stored column wise in this matrix, starting from x1^{-}
71+
// That is, x0^{-} does not exist, that is, the matrix starts from x1^{-}
72+
MatrixXd estimatesApriori;
73+
74+
// this matrix is used to store the a posteriori estimation error covariance matrices Pk^{+}
75+
// note: the matrix starts from P0^{+}=P0, where P0 is the initial guess of the covariance
76+
MatrixXd covarianceAposteriori;
77+
78+
79+
// this matrix is used to store the a priori estimation error covariance matrices Pk^{-}
80+
// note: the matrix starts from P1^{-}, that is, P0^{-} does not exist
81+
MatrixXd covarianceApriori;
82+
83+
// this matrix is used to store the gain matrices Kk
84+
MatrixXd gainMatrices;
85+
86+
// this list is used to store prediction errors error_k=y_k-C*xk^{-}
87+
MatrixXd errors;
88+
89+
};
90+
91+
92+
93+
#endif

0 commit comments

Comments
 (0)