Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020 #ifndef __SR_ITERATED_EXTENDED_KALMAN_FILTER__
00021 #define __SR_ITERATED_EXTENDED_KALMAN_FILTER__
00022
00023 #include "kalmanfilter.h"
00024 #include "../pdf/conditionalpdf.h"
00025 #include "../pdf/gaussian.h"
00026
00027 namespace BFL
00028 {
00029
00053 class SRIteratedExtendedKalmanFilter : public KalmanFilter
00054 {
00055 public:
00061 SRIteratedExtendedKalmanFilter(Gaussian* prior, unsigned int nr_it=1);
00062
00064 virtual ~SRIteratedExtendedKalmanFilter();
00066 virtual void SysUpdate(SystemModel<MatrixWrapper::ColumnVector>* const sysmodel,
00067 const MatrixWrapper::ColumnVector& u);
00069 virtual void SysUpdate(SystemModel<MatrixWrapper::ColumnVector>* const sysmodel);
00071 virtual void MeasUpdate(MeasurementModel<MatrixWrapper::ColumnVector,MatrixWrapper::ColumnVector>* const measmodel,
00072 const MatrixWrapper::ColumnVector& z,
00073 const MatrixWrapper::ColumnVector& s);
00075 virtual void MeasUpdate(MeasurementModel<MatrixWrapper::ColumnVector,MatrixWrapper::ColumnVector>* const measmodel,const MatrixWrapper::ColumnVector& z);
00077 MatrixWrapper::Matrix SRCovarianceGet() const;
00079 void SRCovarianceSet( MatrixWrapper::Matrix JP_new);
00081 void PriorSet(MatrixWrapper::ColumnVector& X, MatrixWrapper::SymmetricMatrix& P);
00082
00091 virtual void CalculateMeasUpdate(MatrixWrapper::ColumnVector z, MatrixWrapper::ColumnVector Z, MatrixWrapper::Matrix H, MatrixWrapper::SymmetricMatrix R);
00092
00094
00099 virtual void CalculateMatrix(MatrixWrapper::Matrix& H_i , MatrixWrapper::SymmetricMatrix& R_i , MatrixWrapper::Matrix& invS , MatrixWrapper::Matrix& Sr , MatrixWrapper::Matrix& K_i );
00100
00102
00105 virtual void CalculateMean(MatrixWrapper::ColumnVector& x_k, const MatrixWrapper::ColumnVector& z, MatrixWrapper::ColumnVector& Z_i ,MatrixWrapper::Matrix& K_i);
00106
00108
00112 virtual void CalculateCovariance( MatrixWrapper::Matrix& R_vf, MatrixWrapper::Matrix& H_i, MatrixWrapper::Matrix& invS, MatrixWrapper::Matrix& SR );
00113
00114 private:
00116 unsigned int nr_iterations;
00118 MatrixWrapper::Matrix JP;
00120
00122
00124
00125 };
00126
00127 }
00128
00129 #endif // __ITERATED_EXTENDED_KALMAN_FILTER__
00130
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 11 2019 03:45:12