00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012 #include "KalmanFilter.h"
00013 #include "KalmanFilterHistory.h"
00014 #include <iostream>
00015 #include <fstream>
00016 using namespace std;
00017
00018 Matrix SMD_Fmatrix(KalmanFilter*);
00019 Matrix SMD_Hmatrix(KalmanFilter*);
00020 Matrix SMD_Qmatrix(KalmanFilter*);
00021 Matrix SMD_Rmatrix(KalmanFilter*);
00022
00023 int main() {
00024
00025
00026 Vector x(2);
00027 Vector z(2);
00028 Vector zeroVector(2);
00029 double tLast = 0.0, t = 0.0;
00030
00031
00032 x(1) = 1.0;
00033
00034
00035 ifstream iFile;
00036 ofstream oFile;
00037
00038
00039 iFile.open("smdmeasurements.txt");
00040 oFile.open("smdresults.txt");
00041
00042
00043 double z1, z2;
00044 iFile >> t >> z1 >> z2;
00045 z(1) = z1; z(2) = z2;
00046
00047
00048 Matrix P(2,2);
00049 P(1,1) = 0.001; P(1,2) = 0.0;
00050 P(2,1) = 0.0; P(2,2) = 0.001;
00051 Matrix K(2,2);
00052 K(1,1) = 1.0; K(1,2) = 0.0;
00053 K(2,1) = 0.0; K(2,2) = 1.0;
00054
00055
00056 KalmanFilter kf(P,K);
00057 KalmanFilterHistory myHist(&kf);
00058
00059
00060
00061 Vector states = x;
00062 Vector controls = zeroVector;
00063 Vector parameters = zeroVector;
00064 Vector measurements = z;
00065
00066 kf.SetTimeOfEstimate(tLast);
00067 kf.SetTimeOfMeasurements(t);
00068 kf.SetStateVector(states);
00069 kf.SetControlVector(controls);
00070 kf.SetMeasurementVector(measurements);
00071 kf.SetParameterVector(parameters);
00072
00073 kf.SetSystemProcessNoiseMatrix(&SMD_Qmatrix);
00074 kf.SetMeasurementCovarianceMatrix(&SMD_Rmatrix);
00075 kf.SetStateJacobianMatrix(&SMD_Fmatrix);
00076 kf.SetMeasurementJacobianMatrix(&SMD_Hmatrix);
00077
00078
00079 oFile << kf.GetStateVector() << endl;
00080
00081 while (iFile) {
00082
00083
00084 iFile >> t >> z1 >> z2;
00085 z(1) = z1; z(2) = z2;
00086
00087
00088 measurements = z;
00089 kf.SetTimeOfMeasurements(t);
00090 kf.SetMeasurementVector(measurements);
00091
00092
00093 kf.EstimateState();
00094
00095
00096 tLast = t;
00097
00098
00099 oFile << kf.GetStateVector() << endl;
00100
00101
00102 myHist.AppendHistory(tLast);
00103
00104 }
00105
00106 Matrix stateData;
00107 Matrix kalmanData;
00108
00109 myHist.GetKalmanHistory(stateData, kalmanData);
00110
00111 cout << "Here comes the history:" << endl << stateData;
00112 char pause;
00113 cout << "Press the any key." << endl;
00114 cin >> pause;
00115
00116 cout << "Here's the rest:" << endl << kalmanData;
00117
00118 iFile.close();
00119 oFile.close();
00120
00121 return 1;
00122
00123 }
00124
00125 Matrix SMD_Fmatrix(KalmanFilter* kf) {
00126
00127 Matrix temp(2,2);
00128 double k = 0.2, b = 0.1, m = 5.0;
00129
00130 temp(1,1) = 0.0; temp(1,2) = 1.0;
00131 temp(2,1) = -k / m; temp(2,2) = -b / m;
00132
00133 return temp;
00134
00135 };
00136
00137 Matrix SMD_Hmatrix(KalmanFilter* kf) {
00138
00139 Matrix temp(2,2);
00140
00141 temp(1,1) = 1.0; temp(1,2) = 0.0;
00142 temp(2,1) = 0.0; temp(2,2) = 1.0;
00143
00144 return temp;
00145
00146 };
00147
00148 Matrix SMD_Qmatrix(KalmanFilter* kf) {
00149
00150 Matrix temp(2,2);
00151
00152 temp(1,1) = 0.001; temp(1,2) = 0.0;
00153 temp(2,1) = 0.0; temp(2,2) = 0.001;
00154
00155 return temp;
00156
00157 };
00158
00159 Matrix SMD_Rmatrix(KalmanFilter* kf) {
00160
00161 Matrix temp(2,2);
00162 double x1_err = 0.2, x2_err = 0.02, x1_std, x2_std;
00163
00164 x1_std = x1_err / pow(2.0, 0.5);
00165 x2_std = x2_err / pow(2.0, 0.5);
00166
00167 temp(1,1) = pow(x1_std, 2.0); temp(1,2) = 0.0;
00168 temp(2,1) = 0.0; temp(2,2) = pow(x1_std, 2.0);
00169
00170 return temp;
00171
00172 };
00173
00174
00175
00176
00177
00178
00179
00180
00181
00182
00183
00184
00185
00186
00187
00188
00189
00190
00191
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212