Bayesian Filtering Library Generated from SVN r
|
00001 // $Id: SRiteratedextendedkalmanfilter.h 29830 2009-01-14 15:10:41Z kgadeyne $ 00002 // Copyright (C) 2002 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 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 //BFL::Matrix invS; 00122 //BFL::Matrix Sr; 00124 //BFL::Matrix K_i; 00125 }; // class 00126 00127 } // End namespace BFL 00128 00129 #endif // __ITERATED_EXTENDED_KALMAN_FILTER__ 00130