SRiteratedextendedkalmanfilter.cpp
Go to the documentation of this file.
1 // $Id$
2 // Copyright (C) 2003 Klaas Gadeyne <first dot last at gmail dot com>
3 // Peter Slaets <peter dot slaets at mech dot kuleuven dot ac dot be>
4 //
5 // This program is free software; you can redistribute it and/or modify
6 // it under the terms of the GNU Lesser General Public License as published by
7 // the Free Software Foundation; either version 2.1 of the License, or
8 // (at your option) any later version.
9 //
10 // This program is distributed in the hope that it will be useful,
11 // but WITHOUT ANY WARRANTY; without even the implied warranty of
12 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 // GNU Lesser General Public License for more details.
14 //
15 // You should have received a copy of the GNU Lesser General Public License
16 // along with this program; if not, write to the Free Software
17 // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
18 //
20 #include "../model/linearanalyticmeasurementmodel_gaussianuncertainty_implicit.h"
21 
22 namespace BFL
23 {
24 using namespace MatrixWrapper;
25 #define AnalyticSys AnalyticSystemModelGaussianUncertainty
26 #define LinearAnalyticMeas_Implicit LinearAnalyticMeasurementModelGaussianUncertainty_Implicit
27 #define Numerical_Limitation 100*100
28 
30  : KalmanFilter(prior),
31  nr_iterations(nr_it), JP(prior->CovarianceGet().rows(),prior->CovarianceGet().rows())
32  {
33  (prior->CovarianceGet()).cholesky_semidefinite(JP);
34  }
35 
37 
38  void
39  SRIteratedExtendedKalmanFilter::SysUpdate(SystemModel<MatrixWrapper::ColumnVector>* const sysmodel,const MatrixWrapper::ColumnVector& u)
40  {
41 
42  MatrixWrapper::ColumnVector x = _post->ExpectedValueGet();
43  MatrixWrapper::ColumnVector J = ((AnalyticSys*)sysmodel)->PredictionGet(u,x);
44  MatrixWrapper::Matrix F = ((AnalyticSys*)sysmodel)->df_dxGet(u,x);
45  MatrixWrapper::SymmetricMatrix Q = ((AnalyticSys*)sysmodel)->CovarianceGet(u,x);
46  // cout<<"JP1\n"<<JP<<endl;
47 
48  CalculateSysUpdate(J, F, Q);
49  // cout<<"JP2\n"<<JP<<endl;
50  // cout<<"post_covar\n"<<_post->CovarianceGet()<<endl;
51 
52  ((_post->CovarianceGet()).cholesky_semidefinite(JP));
53  JP = JP.transpose();
54  // cout<<"JP3\n"<<JP<<endl;
55 
56  }
57 
58  void
60  {
61  MatrixWrapper::ColumnVector u(0);
62  SysUpdate(sysmodel, u);
63  }
64 
65  void
66  SRIteratedExtendedKalmanFilter::MeasUpdate(MeasurementModel<MatrixWrapper::ColumnVector,MatrixWrapper::ColumnVector>* const measmodel,const MatrixWrapper::ColumnVector& z,const MatrixWrapper::ColumnVector& s)
67  {
68 
69  MatrixWrapper::Matrix invS(z.rows(),z.rows());
70  MatrixWrapper::Matrix Sr(z.rows(),z.rows());
71  MatrixWrapper::Matrix K_i(_post->CovarianceGet().rows(),z.rows());
72 
73  MatrixWrapper::ColumnVector x_k = _post->ExpectedValueGet();
74  MatrixWrapper::SymmetricMatrix P_k = _post->CovarianceGet();
75  MatrixWrapper::ColumnVector x_i = _post->ExpectedValueGet();
76 
77  MatrixWrapper::Matrix H_i; MatrixWrapper::SymmetricMatrix R_i;
78  MatrixWrapper::Matrix R_vf; MatrixWrapper::Matrix SR_vf;
79  MatrixWrapper::ColumnVector Z_i;
80  MatrixWrapper::Matrix U; MatrixWrapper::ColumnVector V; MatrixWrapper::Matrix W;
81  MatrixWrapper::Matrix JP1; int change;
82 
83 
84  Matrix diag(JP.rows(),JP.columns());
85  Matrix invdiag(JP.rows(),JP.columns());
86  diag=0;invdiag=0;change=0;
87  V=0;U=0;W=0;
88 
89  // matrix determining the numerical limitations of covariance matrix:
90  for(unsigned int j=1;j<JP.rows()+1;j++){diag(j,j)=100; invdiag(j,j)=0.01;}
91 
92 
93  for (unsigned int i=1; i<nr_iterations+1; i++)
94  {
95  x_i = _post->ExpectedValueGet();
96 
97  H_i = ((LinearAnalyticMeas_Implicit*)measmodel)->df_dxGet(s,x_i);
98  Z_i = ((LinearAnalyticMeas_Implicit*)measmodel)->ExpectedValueGet() + ( H_i * (x_k - x_i) );
99 
100  R_i = ((LinearAnalyticMeas_Implicit*)measmodel)->CovarianceGet();
101  SR_vf = ((LinearAnalyticMeas_Implicit*)measmodel)->SRCovariance();
102 
103  // check two different types of Kalman filters:
104  if(((LinearAnalyticMeas_Implicit*)measmodel)->Is_Identity()==1)
105  {
106  R_vf = SR_vf.transpose();
107  }
108  else
109  {
110  R_i.cholesky_semidefinite(R_vf);
111  R_vf = R_vf.transpose();
112  }
113 
114  // numerical limitations
115  // The singular values of the Square root covariance matrix are limited the the value of 10e-4
116  // because of numerical stabilisation of the Kalman filter algorithm.
117  JP.SVD(V,U,W);
118  MatrixWrapper::Matrix V_matrix(U.columns(),W.columns());
119  for(unsigned int k=1;k<JP.rows()+1;k++)
120  {
121  V_matrix(k,k) = V(k);
122  V(k)=max(V(k),1.0/(Numerical_Limitation));
123  if(V(k)==1/(Numerical_Limitation)){change=1;}
124  }
125  if(change==1)
126  {
127  JP = U*V_matrix*(W.transpose());
128  }
129 
130  // end limitations
131 
132  CalculateMatrix(H_i, R_i , invS , K_i , Sr );
133 
134  CalculateMean(x_k, z, Z_i , K_i);
135 
136  if (i==nr_iterations)
137  {
138  CalculateCovariance( R_vf, H_i, invS, Sr );
139  }
140 
141  }
142  }
143 
144 
145  void
147  const ColumnVector& z)
148  {
149  ColumnVector s(0);
150  MeasUpdate(measmodel, z, s);
151  }
152 
154  {
155  return (Matrix) JP;
156  }
157 
159  {
160  JP=JP_new;
161  }
162 
163  void SRIteratedExtendedKalmanFilter::PriorSet(ColumnVector& X_prior,SymmetricMatrix& P_prior)
164  {
165  PostMuSet( X_prior );
166  PostSigmaSet( P_prior );
167  }
168 void
169  SRIteratedExtendedKalmanFilter::CalculateMeasUpdate(ColumnVector z, ColumnVector Z, Matrix H, SymmetricMatrix R)
170  {
171  // build K matrix
172  Matrix S = ( H * (Matrix)(_post->CovarianceGet()) * (H.transpose()) ) + (Matrix)R;
173  Matrix K = (Matrix)(_post->CovarianceGet()) * (H.transpose()) * (S.inverse());
174 
175  // calcutate new state gaussian
176  ColumnVector Mu_new = ( _post->ExpectedValueGet() + K * (z - Z) );
177  Matrix Sigma_new_matrix = (Matrix)(_post->CovarianceGet()) - K * H * (Matrix)(_post->CovarianceGet());
178  // convert to symmetric matrix
179  SymmetricMatrix Sigma_new(_post->DimensionGet());
180  Sigma_new_matrix.convertToSymmetricMatrix(Sigma_new);
181 
182  // set new state gaussian
183  PostMuSet ( Mu_new );
184  PostSigmaSet( Sigma_new );
185 
186  }
187 void
188  SRIteratedExtendedKalmanFilter::CalculateMatrix(Matrix& H_i, SymmetricMatrix& R_i, Matrix& invS, Matrix& K_i, Matrix& Sr)
189  {
190  MatrixWrapper::Matrix S_i1,S_i2,S_temp1;
191  MatrixWrapper::SymmetricMatrix S_temp2,S_temp;
192  S_i1 = ( H_i * (Matrix)JP * (Matrix) (JP.transpose())* (H_i.transpose()) );
193  S_i2 = (Matrix) R_i;
194  S_temp1 = (S_i1 + S_i2).transpose();
195  S_temp1.convertToSymmetricMatrix(S_temp);
196 
197  S_temp.cholesky_semidefinite(Sr);
198  Sr = Sr.transpose();
199 
200  invS = Sr.inverse();
201  K_i = JP*(JP.transpose())*(H_i.transpose())*(invS.transpose())*invS;
202 
203 /* cout<<"H_i\n"<<H_i<<endl;
204  cout<<"JP\n"<<JP<<endl;
205  cout<<"S_i1\n"<<S_i1<<endl;
206  cout<<"S_i1\n"<<S_i1<<endl;
207  cout<<"S_i2\n"<<S_i2<<endl;
208  cout<<"K_i\n"<<K_i<<endl;
209  */
210  }
211 
212 void
213  SRIteratedExtendedKalmanFilter::CalculateMean(ColumnVector& x_k, const ColumnVector& z, ColumnVector& Z_i ,Matrix& K_i)
214  {
215  MatrixWrapper::ColumnVector x_i;
216  x_i = x_k + K_i * (z - Z_i);
217  PostMuSet( x_i );
218  }
219 
220 void
221  SRIteratedExtendedKalmanFilter::CalculateCovariance(Matrix& R_vf, Matrix& H_i, Matrix& invS ,Matrix& Sr)
222  {
223  MatrixWrapper::Matrix temp;
224  temp = (Matrix)R_vf+(Matrix)Sr;
225  JP = (Matrix)JP -(Matrix)JP*(Matrix)(JP.transpose()) * (H_i.transpose()) * (Matrix)(invS.transpose())*(Matrix)(temp.inverse())*H_i*(Matrix)JP;
226  MatrixWrapper::SymmetricMatrix Sigma;
227  MatrixWrapper::Matrix Sigma1;
228  Sigma1=(JP*(JP.transpose())).transpose();
229  Sigma1.convertToSymmetricMatrix(Sigma);
230  PostSigmaSet(Sigma);
231  }
232 }
void PostMuSet(const MatrixWrapper::ColumnVector &c)
Set expected value of posterior estimate.
virtual void CalculateMeasUpdate(MatrixWrapper::ColumnVector z, MatrixWrapper::ColumnVector Z, MatrixWrapper::Matrix H, MatrixWrapper::SymmetricMatrix R)
MatrixWrapper::Matrix SRCovarianceGet() const
Returns a square root of the covariance of the measurement input u.
Class representing Gaussian (or normal density)
Definition: gaussian.h:27
void PostSigmaSet(const MatrixWrapper::SymmetricMatrix &s)
Set covariance of posterior estimate.
Pdf< MatrixWrapper::ColumnVector > * _post
Pointer to the Posterior Pdf.
Definition: filter.h:95
void SRCovarianceSet(MatrixWrapper::Matrix JP_new)
Set the square root covariance to a specific value.
virtual MatrixWrapper::SymmetricMatrix CovarianceGet() const
Get the Covariance Matrix E[(x - E[x])^2] of the Analytic pdf.
Definition: gaussian.cpp:187
MatrixWrapper::Matrix JP
the upper triangular matrix cholesky decompostion of the state covariance ( )
SRIteratedExtendedKalmanFilter(Gaussian *prior, unsigned int nr_it=1)
void PriorSet(MatrixWrapper::ColumnVector &X, MatrixWrapper::SymmetricMatrix &P)
Set mean and covariance of the state estimation to a specific value.
virtual void MeasUpdate(MeasurementModel< MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector > *const measmodel, const MatrixWrapper::ColumnVector &z, const MatrixWrapper::ColumnVector &s)
Perform a measurement update use a measurement model, measurements z and virutal measurements s...
#define LinearAnalyticMeas_Implicit
Class representing the family of all Kalman Filters (EKF, IEKF, ...)
Definition: kalmanfilter.h:49
virtual void SysUpdate(SystemModel< MatrixWrapper::ColumnVector > *const sysmodel, const MatrixWrapper::ColumnVector &u)
Perform a system update with the current measurement model ans system model using an input u...
virtual void CalculateMean(MatrixWrapper::ColumnVector &x_k, const MatrixWrapper::ColumnVector &z, MatrixWrapper::ColumnVector &Z_i, MatrixWrapper::Matrix &K_i)
Calculate the new state estimate.
unsigned int DimensionGet() const
Get the dimension of the argument.
void CalculateSysUpdate(const MatrixWrapper::ColumnVector &J, const MatrixWrapper::Matrix &F, const MatrixWrapper::SymmetricMatrix &Q)
virtual T ExpectedValueGet() const
Get the expected value E[x] of the pdf.
unsigned int nr_iterations
Variable indicating the number of iterations of the filter.
#define Numerical_Limitation
virtual void CalculateMatrix(MatrixWrapper::Matrix &H_i, MatrixWrapper::SymmetricMatrix &R_i, MatrixWrapper::Matrix &invS, MatrixWrapper::Matrix &Sr, MatrixWrapper::Matrix &K_i)
Calculate K_i , invS and Sr.
virtual void CalculateCovariance(MatrixWrapper::Matrix &R_vf, MatrixWrapper::Matrix &H_i, MatrixWrapper::Matrix &invS, MatrixWrapper::Matrix &SR)
Calculate the covariance of the new state estimate (P_k)
virtual MatrixWrapper::SymmetricMatrix CovarianceGet() const
Get the Covariance Matrix E[(x - E[x])^2] of the Analytic pdf.


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 Mon Feb 28 2022 21:56:33