Bayesian Filtering Library  Generated from SVN r
SRiteratedextendedkalmanfilter.h
1 // $Id$
2 // Copyright (C) 2002 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 //
19 
20 #ifndef __SR_ITERATED_EXTENDED_KALMAN_FILTER__
21 #define __SR_ITERATED_EXTENDED_KALMAN_FILTER__
22 
23 #include "kalmanfilter.h"
24 #include "../pdf/conditionalpdf.h"
25 #include "../pdf/gaussian.h"
26 
27 namespace BFL
28 {
29 
54  {
55  public:
61  SRIteratedExtendedKalmanFilter(Gaussian* prior, unsigned int nr_it=1);
62 
66  virtual void SysUpdate(SystemModel<MatrixWrapper::ColumnVector>* const sysmodel,
69  virtual void SysUpdate(SystemModel<MatrixWrapper::ColumnVector>* const sysmodel);
82 
92 
94 
100 
102 
106 
108 
113 
114  private:
116  unsigned int nr_iterations;
120  //BFL::Matrix invS;
122  //BFL::Matrix Sr;
124  //BFL::Matrix K_i;
125  }; // class
126 
127 } // End namespace BFL
128 
129 #endif // __ITERATED_EXTENDED_KALMAN_FILTER__
130 
SRIteratedExtendedKalmanFilter(Gaussian *prior, unsigned int nr_it=1)
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...
Class representing Gaussian (or normal density)
Definition: gaussian.h:27
void PriorSet(MatrixWrapper::ColumnVector &X, MatrixWrapper::SymmetricMatrix &P)
Set mean and covariance of the state estimation to a specific value.
Wrapper class for ColumnVectors (Boost implementation)
Definition: asirfilter.h:41
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 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...
Implementation of Matrixwrapper using Boost.
Definition: asirfilter.h:46
Class representing the family of all Kalman Filters (EKF, IEKF, ...)
Definition: kalmanfilter.h:49
MatrixWrapper::Matrix SRCovarianceGet() const
Returns a square root of the covariance of the measurement input u.
virtual void CalculateMeasUpdate(MatrixWrapper::ColumnVector z, MatrixWrapper::ColumnVector Z, MatrixWrapper::Matrix H, MatrixWrapper::SymmetricMatrix R)
virtual void CalculateMean(MatrixWrapper::ColumnVector &x_k, const MatrixWrapper::ColumnVector &z, MatrixWrapper::ColumnVector &Z_i, MatrixWrapper::Matrix &K_i)
Calculate the new state estimate.
void SRCovarianceSet(MatrixWrapper::Matrix JP_new)
Set the square root covariance to a specific value.
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 ~SRIteratedExtendedKalmanFilter()
Destructor.