00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012 #include "ExtendedKalmanFilter.h"
00013 #include <iostream>
00014 #include <fstream>
00015 using namespace std;
00016
00017 Matrix Euler_Fmatrix(KalmanFilter*);
00018 Matrix Euler_Hmatrix(KalmanFilter*);
00019 Matrix Euler_Qmatrix(KalmanFilter*);
00020 Matrix Euler_Rmatrix(KalmanFilter*);
00021
00022 Vector Euler_stateRHSFunc(double t, Vector initialState, Vector controls, Vector params);
00023 Vector Euler_measurementRHSFunc(ExtendedKalmanFilter* ekf);
00024
00025 int main() {
00026
00027
00028 Vector x(3);
00029 Vector z(3);
00030 Vector zeroVector(3);
00031 double tLast = 0.0, t = 0.0;
00032
00033
00034 x(1) = 0.1745;
00035 x(2) = 0.1745;
00036 x(3) = 0.1745;
00037 Vector p(3);
00038 p(1) = 2.0;
00039 p(2) = 3.0;
00040 p(3) = 4.0;
00041
00042
00043 ifstream iFile;
00044 ofstream oFile;
00045
00046
00047 iFile.open("eulermeasurements.txt");
00048 oFile.open("eulerresults.txt");
00049
00050
00051 double z1, z2, z3;
00052 iFile >> t >> z1 >> z2 >> z3;
00053 z(1) = z1; z(2) = z2; z(3) = z3;
00054
00055
00056 Matrix P(3,3);
00057 P(1,1) = 0.001; P(1,2) = 0.0; P(1,3) = 0.0;
00058 P(2,1) = 0.0; P(2,2) = 0.001; P(2,3) = 0.0;
00059 P(3,1) = 0.0; P(3,2) = 0.0; P(3,3) = 0.001;
00060 Matrix K(3,3);
00061 K(1,1) = 0.001; K(1,2) = 0.0; K(1,3) = 0.0;
00062 K(2,1) = 0.0; K(2,2) = 0.001; K(2,3) = 0.0;
00063 K(3,1) = 0.0; K(3,2) = 0.0; K(3,3) = 0.001;
00064
00065
00066 ExtendedKalmanFilter ekf;
00067
00068 ekf.SetCovarianceMatrix(P);
00069 ekf.SetKalmanGainMatrix(K);
00070
00071
00072 Vector states = x;
00073 Vector controls = zeroVector;
00074 Vector parameters = p;
00075 Vector measurements = z;
00076
00077 ekf.SetTimeOfEstimate(tLast);
00078 ekf.SetTimeOfMeasurements(t);
00079 ekf.SetStateVector(states);
00080 ekf.SetControlVector(controls);
00081 ekf.SetMeasurementVector(measurements);
00082 ekf.SetParameterVector(parameters);
00083
00084 ekf.SetSystemProcessNoiseMatrix(&Euler_Qmatrix);
00085 ekf.SetMeasurementCovarianceMatrix(&Euler_Rmatrix);
00086 ekf.SetStateJacobianMatrix(&Euler_Fmatrix);
00087 ekf.SetMeasurementJacobianMatrix(&Euler_Hmatrix);
00088
00089 ekf.SetStateRHS(&Euler_stateRHSFunc);
00090 ekf.SetMeasurementRHS(&Euler_measurementRHSFunc);
00091
00092
00093 oFile << t << " " << ~ekf.GetStateVector();
00094
00095 while (iFile) {
00096
00097
00098 iFile >> t >> z1 >> z2 >> z3;
00099 z(1) = z1; z(2) = z2; z(3) = z3;
00100
00101
00102 measurements = z;
00103 ekf.SetTimeOfMeasurements(t);
00104 ekf.SetMeasurementVector(measurements);
00105
00106
00107 ekf.EstimateState();
00108
00109
00110 tLast = t;
00111
00112
00113 oFile << t << " " << ~ekf.GetStateVector();
00114
00115 }
00116
00117 cout << "I made it all the way through!" << endl;
00118
00119 iFile.close();
00120 oFile.close();
00121
00122 return 1;
00123
00124 }
00125
00126 Matrix Euler_Fmatrix(KalmanFilter* ekf) {
00127
00128 Matrix temp(3,3);
00129
00130 Vector state = ekf->GetStateVector();
00131 Vector params = ekf->GetParameterVector();
00132
00133 double w1 = state(1), w2 = state(2), w3 = state(3),
00134 I1 = params(1), I2 = params(2), I3 = params(3);
00135
00136 temp(1,1) = 0.0; temp(1,2) = (I2-I3)/I1 * w3; temp(1,3) = (I2-I3)/I1 * w2;
00137 temp(2,1) = (I3-I1)/I2 * w3; temp(2,2) = 0.0; temp(2,3) = (I3-I1)/I2 * w1;
00138 temp(3,1) = (I1-I2)/I3 * w1; temp(3,2) = (I1-I2)/I3 * w1; temp(3,3) = 0.0;
00139
00140 return temp;
00141
00142 };
00143
00144 Matrix Euler_Hmatrix(KalmanFilter* kf) {
00145
00146 Matrix temp(3,3);
00147
00148 temp(1,1) = 1.0; temp(1,2) = 0.0; temp(1,3) = 0.0;
00149 temp(2,1) = 0.0; temp(2,2) = 1.0; temp(2,3) = 0.0;
00150 temp(3,1) = 0.0; temp(3,2) = 0.0; temp(3,3) = 1.0;
00151
00152 return temp;
00153
00154 };
00155
00156 Matrix Euler_Qmatrix(KalmanFilter* kf) {
00157
00158 Matrix temp(3,3);
00159
00160 temp(1,1) = 0.001; temp(1,2) = 0.0; temp(1,3) = 0.0;
00161 temp(2,1) = 0.0; temp(2,2) = 0.001; temp(2,3) = 0.0;
00162 temp(3,1) = 0.0; temp(3,2) = 0.0; temp(3,3) = 0.001;
00163
00164 return temp;
00165
00166 };
00167
00168 Matrix Euler_Rmatrix(KalmanFilter* ekf) {
00169
00170 Matrix temp(3,3);
00171 double gyro_err = 0.005, gyro_std = gyro_err / pow(2.0, 0.5);
00172
00173 temp(1,1) = pow(gyro_std, 2.0); temp(1,2) = 0.0; temp(1,3) = 0.0;
00174 temp(2,1) = 0.0; temp(2,2) = pow(gyro_std, 2.0); temp(2,3) = 0.0;
00175 temp(3,1) = 0.0; temp(3,2) = 0.0; temp(3,3) = pow(gyro_std, 2.0);
00176
00177 return temp;
00178
00179 };
00180
00181 Vector Euler_stateRHSFunc(double t, Vector initialState, Vector controls, Vector params) {
00182
00183 Vector w = initialState;
00184
00185 Matrix MOI(3,3);
00186
00187 MOI(1,1) = params(1); MOI(1,2) = 0.0; MOI(1,3) = 0.0;
00188 MOI(2,1) = 0.0; MOI(2,2) = params(2); MOI(2,3) = 0.0;
00189 MOI(3,1) = 0.0; MOI(3,2) = 0.0; MOI(3,3) = params(3);
00190
00191 Matrix MOIinv(3,3);
00192
00193 MOIinv(1,1) = 1.0/params(1); MOIinv(1,2) = 0.0; MOIinv(1,3) = 0.0;
00194 MOIinv(2,1) = 0.0; MOIinv(2,2) = 1.0/params(2); MOIinv(2,3) = 0.0;
00195 MOIinv(3,1) = 0.0; MOIinv(3,2) = 0.0; MOIinv(3,3) = 1.0/params(3);
00196
00197 Matrix wsk(3,3);
00198
00199 wsk(1,1) = 0.0; wsk(1,2) = -w(3); wsk(1,3) = w(2);
00200 wsk(2,1) = w(3); wsk(2,2) = 0.0; wsk(2,3) = -w(1);
00201 wsk(3,1) = -w(2); wsk(3,2) = w(1); wsk(3,3) = 0.0;
00202
00203 return (MOIinv * (-wsk * MOI * w));
00204
00205 };
00206
00207 Vector Euler_measurementRHSFunc(ExtendedKalmanFilter* ekf) { return ekf->GetStateVector(); };
00208
00209
00210
00211
00212
00213
00214
00215
00216
00217
00218
00219
00220
00221
00222
00223
00224
00225
00226
00227
00228
00229
00230
00231
00232
00233
00234
00235
00236
00237
00238
00239
00240
00241
00242
00243
00244
00245
00246
00247