00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019 #include "SRiteratedextendedkalmanfilter.h"
00020 #include "../model/linearanalyticmeasurementmodel_gaussianuncertainty_implicit.h"
00021
00022 namespace BFL
00023 {
00024 using namespace MatrixWrapper;
00025 #define AnalyticSys AnalyticSystemModelGaussianUncertainty
00026 #define LinearAnalyticMeas_Implicit LinearAnalyticMeasurementModelGaussianUncertainty_Implicit
00027 #define Numerical_Limitation 100*100
00028
00029 SRIteratedExtendedKalmanFilter::SRIteratedExtendedKalmanFilter(Gaussian* prior, unsigned int nr_it)
00030 : KalmanFilter(prior),
00031 nr_iterations(nr_it), JP(prior->CovarianceGet().rows(),prior->CovarianceGet().rows())
00032 {
00033 (prior->CovarianceGet()).cholesky_semidefinite(JP);
00034 }
00035
00036 SRIteratedExtendedKalmanFilter::~SRIteratedExtendedKalmanFilter(){}
00037
00038 void
00039 SRIteratedExtendedKalmanFilter::SysUpdate(SystemModel<MatrixWrapper::ColumnVector>* const sysmodel,const MatrixWrapper::ColumnVector& u)
00040 {
00041
00042 MatrixWrapper::ColumnVector x = _post->ExpectedValueGet();
00043 MatrixWrapper::ColumnVector J = ((AnalyticSys*)sysmodel)->PredictionGet(u,x);
00044 MatrixWrapper::Matrix F = ((AnalyticSys*)sysmodel)->df_dxGet(u,x);
00045 MatrixWrapper::SymmetricMatrix Q = ((AnalyticSys*)sysmodel)->CovarianceGet(u,x);
00046
00047
00048 CalculateSysUpdate(J, F, Q);
00049
00050
00051
00052 ((_post->CovarianceGet()).cholesky_semidefinite(JP));
00053 JP = JP.transpose();
00054
00055
00056 }
00057
00058 void
00059 SRIteratedExtendedKalmanFilter::SysUpdate(SystemModel<MatrixWrapper::ColumnVector>* const sysmodel)
00060 {
00061 MatrixWrapper::ColumnVector u(0);
00062 SysUpdate(sysmodel, u);
00063 }
00064
00065 void
00066 SRIteratedExtendedKalmanFilter::MeasUpdate(MeasurementModel<MatrixWrapper::ColumnVector,MatrixWrapper::ColumnVector>* const measmodel,const MatrixWrapper::ColumnVector& z,const MatrixWrapper::ColumnVector& s)
00067 {
00068
00069 MatrixWrapper::Matrix invS(z.rows(),z.rows());
00070 MatrixWrapper::Matrix Sr(z.rows(),z.rows());
00071 MatrixWrapper::Matrix K_i(_post->CovarianceGet().rows(),z.rows());
00072
00073 MatrixWrapper::ColumnVector x_k = _post->ExpectedValueGet();
00074 MatrixWrapper::SymmetricMatrix P_k = _post->CovarianceGet();
00075 MatrixWrapper::ColumnVector x_i = _post->ExpectedValueGet();
00076
00077 MatrixWrapper::Matrix H_i; MatrixWrapper::SymmetricMatrix R_i;
00078 MatrixWrapper::Matrix R_vf; MatrixWrapper::Matrix SR_vf;
00079 MatrixWrapper::ColumnVector Z_i;
00080 MatrixWrapper::Matrix U; MatrixWrapper::ColumnVector V; MatrixWrapper::Matrix W;
00081 MatrixWrapper::Matrix JP1; int change;
00082
00083
00084 Matrix diag(JP.rows(),JP.columns());
00085 Matrix invdiag(JP.rows(),JP.columns());
00086 diag=0;invdiag=0;change=0;
00087 V=0;U=0;W=0;
00088
00089
00090 for(unsigned int j=1;j<JP.rows()+1;j++){diag(j,j)=100; invdiag(j,j)=0.01;}
00091
00092
00093 for (unsigned int i=1; i<nr_iterations+1; i++)
00094 {
00095 x_i = _post->ExpectedValueGet();
00096
00097 H_i = ((LinearAnalyticMeas_Implicit*)measmodel)->df_dxGet(s,x_i);
00098 Z_i = ((LinearAnalyticMeas_Implicit*)measmodel)->ExpectedValueGet() + ( H_i * (x_k - x_i) );
00099
00100 R_i = ((LinearAnalyticMeas_Implicit*)measmodel)->CovarianceGet();
00101 SR_vf = ((LinearAnalyticMeas_Implicit*)measmodel)->SRCovariance();
00102
00103
00104 if(((LinearAnalyticMeas_Implicit*)measmodel)->Is_Identity()==1)
00105 {
00106 R_vf = SR_vf.transpose();
00107 }
00108 else
00109 {
00110 R_i.cholesky_semidefinite(R_vf);
00111 R_vf = R_vf.transpose();
00112 }
00113
00114
00115
00116
00117 JP.SVD(V,U,W);
00118 MatrixWrapper::Matrix V_matrix(U.columns(),W.columns());
00119 for(unsigned int k=1;k<JP.rows()+1;k++)
00120 {
00121 V_matrix(k,k) = V(k);
00122 V(k)=max(V(k),1.0/(Numerical_Limitation));
00123 if(V(k)==1/(Numerical_Limitation)){change=1;}
00124 }
00125 if(change==1)
00126 {
00127 JP = U*V_matrix*(W.transpose());
00128 }
00129
00130
00131
00132 CalculateMatrix(H_i, R_i , invS , K_i , Sr );
00133
00134 CalculateMean(x_k, z, Z_i , K_i);
00135
00136 if (i==nr_iterations)
00137 {
00138 CalculateCovariance( R_vf, H_i, invS, Sr );
00139 }
00140
00141 }
00142 }
00143
00144
00145 void
00146 SRIteratedExtendedKalmanFilter::MeasUpdate(MeasurementModel<ColumnVector,ColumnVector>* const measmodel,
00147 const ColumnVector& z)
00148 {
00149 ColumnVector s(0);
00150 MeasUpdate(measmodel, z, s);
00151 }
00152
00153 Matrix SRIteratedExtendedKalmanFilter::SRCovarianceGet() const
00154 {
00155 return (Matrix) JP;
00156 }
00157
00158 void SRIteratedExtendedKalmanFilter::SRCovarianceSet(Matrix JP_new)
00159 {
00160 JP=JP_new;
00161 }
00162
00163 void SRIteratedExtendedKalmanFilter::PriorSet(ColumnVector& X_prior,SymmetricMatrix& P_prior)
00164 {
00165 PostMuSet( X_prior );
00166 PostSigmaSet( P_prior );
00167 }
00168 void
00169 SRIteratedExtendedKalmanFilter::CalculateMeasUpdate(ColumnVector z, ColumnVector Z, Matrix H, SymmetricMatrix R)
00170 {
00171
00172 Matrix S = ( H * (Matrix)(_post->CovarianceGet()) * (H.transpose()) ) + (Matrix)R;
00173 Matrix K = (Matrix)(_post->CovarianceGet()) * (H.transpose()) * (S.inverse());
00174
00175
00176 ColumnVector Mu_new = ( _post->ExpectedValueGet() + K * (z - Z) );
00177 Matrix Sigma_new_matrix = (Matrix)(_post->CovarianceGet()) - K * H * (Matrix)(_post->CovarianceGet());
00178
00179 SymmetricMatrix Sigma_new(_post->DimensionGet());
00180 Sigma_new_matrix.convertToSymmetricMatrix(Sigma_new);
00181
00182
00183 PostMuSet ( Mu_new );
00184 PostSigmaSet( Sigma_new );
00185
00186 }
00187 void
00188 SRIteratedExtendedKalmanFilter::CalculateMatrix(Matrix& H_i, SymmetricMatrix& R_i, Matrix& invS, Matrix& K_i, Matrix& Sr)
00189 {
00190 MatrixWrapper::Matrix S_i1,S_i2,S_temp1;
00191 MatrixWrapper::SymmetricMatrix S_temp2,S_temp;
00192 S_i1 = ( H_i * (Matrix)JP * (Matrix) (JP.transpose())* (H_i.transpose()) );
00193 S_i2 = (Matrix) R_i;
00194 S_temp1 = (S_i1 + S_i2).transpose();
00195 S_temp1.convertToSymmetricMatrix(S_temp);
00196
00197 S_temp.cholesky_semidefinite(Sr);
00198 Sr = Sr.transpose();
00199
00200 invS = Sr.inverse();
00201 K_i = JP*(JP.transpose())*(H_i.transpose())*(invS.transpose())*invS;
00202
00203
00204
00205
00206
00207
00208
00209
00210 }
00211
00212 void
00213 SRIteratedExtendedKalmanFilter::CalculateMean(ColumnVector& x_k, const ColumnVector& z, ColumnVector& Z_i ,Matrix& K_i)
00214 {
00215 MatrixWrapper::ColumnVector x_i;
00216 x_i = x_k + K_i * (z - Z_i);
00217 PostMuSet( x_i );
00218 }
00219
00220 void
00221 SRIteratedExtendedKalmanFilter::CalculateCovariance(Matrix& R_vf, Matrix& H_i, Matrix& invS ,Matrix& Sr)
00222 {
00223 MatrixWrapper::Matrix temp;
00224 temp = (Matrix)R_vf+(Matrix)Sr;
00225 JP = (Matrix)JP -(Matrix)JP*(Matrix)(JP.transpose()) * (H_i.transpose()) * (Matrix)(invS.transpose())*(Matrix)(temp.inverse())*H_i*(Matrix)JP;
00226 MatrixWrapper::SymmetricMatrix Sigma;
00227 MatrixWrapper::Matrix Sigma1;
00228 Sigma1=(JP*(JP.transpose())).transpose();
00229 Sigma1.convertToSymmetricMatrix(Sigma);
00230 PostSigmaSet(Sigma);
00231 }
00232 }