00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012 #include "ExtendedKalmanFilter.h"
00013 #include "KalmanFilterHistory.h"
00014 #include "utils/Time.h"
00015 #include <iostream>
00016 #include <fstream>
00017 using namespace std;
00018 using namespace O_SESSAME;
00019
00020 Matrix state_Euler_Fmatrix(KalmanFilter*);
00021 Matrix state_Euler_Hmatrix(KalmanFilter*);
00022 Matrix state_Euler_Qmatrix(KalmanFilter*);
00023 Matrix state_Euler_Rmatrix(KalmanFilter*);
00024
00025 Vector state_Euler_stateRHSFunc(double t, Vector initialState, Vector controls, Vector params);
00026 Vector state_Euler_measurementRHSFunc(ExtendedKalmanFilter* ekf);
00027
00028 int main() {
00029
00030 tick();
00031
00032
00033 Vector states(3);
00034 Vector parameters(6);
00035 Vector measurements(3);
00036 Vector controls(3);
00037 double t = 0.0;
00038
00039
00040 states(1) = 0.25;
00041 states(2) = 0.3;
00042 states(3) = 0.2;
00043
00044 parameters(1) = 5.7;
00045 parameters(2) = 0.4;
00046 parameters(3) = 0.3;
00047 parameters(4) = 5.3;
00048 parameters(5) = 0.45;
00049 parameters(6) = 9.7;
00050
00051
00052
00053 ifstream iFile;
00054 ofstream oFile;
00055
00056
00057 iFile.open("DataFiles/eulermeasurements.txt");
00058 oFile.open("DataFiles/EKFomega_bestguess.txt");
00059
00060
00061
00062 iFile >> t >> measurements(1) >> measurements(2) >> measurements(3);
00063
00064
00065
00066
00067 Matrix covariance = eye(3);
00068 covariance *= 0.5;
00069
00070 Matrix kalmanGain(3,3);
00071
00072
00073
00074 ExtendedKalmanFilter state_ekf;
00075 KalmanFilterHistory state_ekfhist(&state_ekf);
00076
00077 state_ekf.SetCovarianceMatrix(covariance);
00078 state_ekf.SetKalmanGainMatrix(kalmanGain);
00079
00080 state_ekf.SetTimeOfEstimate(t);
00081 state_ekf.SetTimeOfMeasurements(t);
00082 state_ekf.SetStateVector(states);
00083 state_ekf.SetControlVector(controls);
00084 state_ekf.SetMeasurementVector(measurements);
00085 state_ekf.SetParameterVector(parameters);
00086
00087 state_ekf.SetSystemProcessNoiseMatrix(&state_Euler_Qmatrix);
00088 state_ekf.SetMeasurementCovarianceMatrix(&state_Euler_Rmatrix);
00089 state_ekf.SetStateJacobianMatrix(&state_Euler_Fmatrix);
00090 state_ekf.SetMeasurementJacobianMatrix(&state_Euler_Hmatrix);
00091
00092 state_ekf.SetStateRHS(&state_Euler_stateRHSFunc);
00093 state_ekf.SetMeasurementRHS(&state_Euler_measurementRHSFunc);
00094
00095 state_ekfhist.AppendHistory(t);
00096
00097
00098
00099
00100 while (iFile) {
00101
00102
00103 iFile >> t >> measurements(1) >> measurements(2) >> measurements(3);
00104
00105
00106 state_ekf.SetTimeOfMeasurements(t);
00107 state_ekf.SetMeasurementVector(measurements);
00108
00109
00110 state_ekf.EstimateState();
00111
00112
00113 state_ekfhist.AppendHistory(t);
00114
00115 }
00116
00117 oFile << state_ekfhist.GetHistory();
00118
00119 cout << "Run completed @ " << tock() << endl;
00120
00121 iFile.close();
00122 oFile.close();
00123
00124 return 1;
00125
00126 }
00127
00128
00129
00130
00131
00132
00133 Vector state_Euler_stateRHSFunc(double t, Vector initialState, Vector controls, Vector params) {
00134
00135
00136 Matrix MOI(3,3);
00137 MOI(1,1) = params(1); MOI(1,2) = params(2); MOI(1,3) = params(3);
00138 MOI(2,1) = params(2); MOI(2,2) = params(4); MOI(2,3) = params(5);
00139 MOI(3,1) = params(3); MOI(3,2) = params(5); MOI(3,3) = params(6);
00140
00141
00142 Vector omega(3);
00143 omega = initialState;
00144
00145
00146 Vector updatedState(3);
00147 updatedState = MOI/( -skew(omega) * MOI * omega );
00148
00149 return (updatedState);
00150
00151 };
00152
00153
00154
00155
00156 Vector state_Euler_measurementRHSFunc(ExtendedKalmanFilter* ekf) {
00157
00158 Vector temp(3);
00159 temp = ekf->GetStateVector();
00160
00161 return(temp);
00162
00163 };
00164
00165
00166
00167 Matrix state_Euler_Fmatrix(KalmanFilter* ekf) {
00168
00169 Vector state = ekf->GetStateVector();
00170 Vector params = ekf->GetParameterVector();
00171
00172
00173 Matrix MOI(3,3);
00174 MOI(1,1) = params(1); MOI(1,2) = params(2); MOI(1,3) = params(3);
00175 MOI(2,1) = params(2); MOI(2,2) = params(4); MOI(2,3) = params(5);
00176 MOI(3,1) = params(3); MOI(3,2) = params(5); MOI(3,3) = params(6);
00177
00178
00179 Vector omega(3);
00180 omega = state;
00181
00182
00183 Matrix temp(3,3);
00184 temp = MOI/( -skew(omega) * MOI + skew(MOI*omega) );
00185
00186 return temp;
00187
00188 };
00189
00190 Matrix state_Euler_Hmatrix(KalmanFilter* ekf) {
00191
00192 Matrix temp(3,3);
00193 temp = eye(3);
00194
00195 return temp;
00196
00197 };
00198
00199 Matrix state_Euler_Qmatrix(KalmanFilter* ekf) {
00200
00201 Matrix temp = eye(3);
00202 temp *= 0.002;
00203
00204 return temp;
00205
00206 };
00207
00208 Matrix state_Euler_Rmatrix(KalmanFilter* ekf) {
00209
00210 Matrix temp = eye(3);
00211 double gyro_err = 1.0, gyro_std = gyro_err / pow(2.0, 0.5);
00212
00213 temp *= pow(gyro_std, 2.0);
00214
00215 return temp;
00216
00217 };
00218
00219
00220
00221
00222
00223
00224
00225
00226
00227
00228
00229
00230
00231
00232
00233
00234
00235
00236
00237
00238
00239