SRiteratedextendedkalmanfilter.h
Go to the documentation of this file.
00001 // $Id$
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 


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