SRiteratedextendedkalmanfilter.cpp
Go to the documentation of this file.
00001 // $Id: SRiteratedextendedkalmanfilter.cpp 29495 2008-08-13 12:57:49Z tdelaet $
00002 // Copyright (C) 2003 Klaas Gadeyne <first dot last at gmail dot com>
00003 //                    Peter Slaets  <peter dot slaets at mech dot kuleuven dot ac dot be>
00004 //
00005 // This program is free software; you can redistribute it and/or modify
00006 // it under the terms of the GNU Lesser General Public License as published by
00007 // the Free Software Foundation; either version 2.1 of the License, or
00008 // (at your option) any later version.
00009 //
00010 // This program is distributed in the hope that it will be useful,
00011 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00012 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00013 // GNU Lesser General Public License for more details.
00014 //
00015 // You should have received a copy of the GNU Lesser General Public License
00016 // along with this program; if not, write to the Free Software
00017 // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
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   //  cout<<"JP1\n"<<JP<<endl;
00047 
00048     CalculateSysUpdate(J, F, Q);
00049   //  cout<<"JP2\n"<<JP<<endl;
00050    // cout<<"post_covar\n"<<_post->CovarianceGet()<<endl;
00051 
00052     ((_post->CovarianceGet()).cholesky_semidefinite(JP));
00053     JP = JP.transpose();
00054     // cout<<"JP3\n"<<JP<<endl;
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     // matrix determining the numerical limitations of covariance matrix:
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         // check two different types of Kalman filters:
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         // numerical limitations
00115         // The singular values of the Square root covariance matrix are limited the the value of 10e-4
00116         // because of numerical stabilisation of the Kalman filter algorithm.
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         // end limitations
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     // build K matrix
00172     Matrix S = ( H * (Matrix)(_post->CovarianceGet()) * (H.transpose()) ) + (Matrix)R;
00173     Matrix K = (Matrix)(_post->CovarianceGet()) * (H.transpose()) * (S.inverse());
00174 
00175       // calcutate new state gaussian
00176     ColumnVector Mu_new  = ( _post->ExpectedValueGet() + K * (z - Z)  );
00177     Matrix Sigma_new_matrix = (Matrix)(_post->CovarianceGet()) - K * H * (Matrix)(_post->CovarianceGet());
00178     // convert to symmetric matrix
00179     SymmetricMatrix Sigma_new(_post->DimensionGet());
00180     Sigma_new_matrix.convertToSymmetricMatrix(Sigma_new);
00181 
00182     // set new state gaussian
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 /*        cout<<"H_i\n"<<H_i<<endl;
00204           cout<<"JP\n"<<JP<<endl;
00205           cout<<"S_i1\n"<<S_i1<<endl;
00206           cout<<"S_i1\n"<<S_i1<<endl;
00207           cout<<"S_i2\n"<<S_i2<<endl;
00208           cout<<"K_i\n"<<K_i<<endl;
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 }


bfl
Author(s): Klaas Gadeyne, Wim Meeussen, Tinne Delaet and many others. See web page for a full contributor list. ROS package maintained by Wim Meeussen.
autogenerated on Fri Aug 28 2015 10:10:21