SRiteratedextendedkalmanfilter.h
Go to the documentation of this file.
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,
67  const MatrixWrapper::ColumnVector& u);
69  virtual void SysUpdate(SystemModel<MatrixWrapper::ColumnVector>* const sysmodel);
72  const MatrixWrapper::ColumnVector& z,
73  const MatrixWrapper::ColumnVector& s);
75  virtual void MeasUpdate(MeasurementModel<MatrixWrapper::ColumnVector,MatrixWrapper::ColumnVector>* const measmodel,const MatrixWrapper::ColumnVector& z);
77  MatrixWrapper::Matrix SRCovarianceGet() const;
79  void SRCovarianceSet( MatrixWrapper::Matrix JP_new);
81  void PriorSet(MatrixWrapper::ColumnVector& X, MatrixWrapper::SymmetricMatrix& P);
82 
91  virtual void CalculateMeasUpdate(MatrixWrapper::ColumnVector z, MatrixWrapper::ColumnVector Z, MatrixWrapper::Matrix H, MatrixWrapper::SymmetricMatrix R);
92 
94 
99  virtual void CalculateMatrix(MatrixWrapper::Matrix& H_i , MatrixWrapper::SymmetricMatrix& R_i , MatrixWrapper::Matrix& invS , MatrixWrapper::Matrix& Sr , MatrixWrapper::Matrix& K_i );
100 
102 
105  virtual void CalculateMean(MatrixWrapper::ColumnVector& x_k, const MatrixWrapper::ColumnVector& z, MatrixWrapper::ColumnVector& Z_i ,MatrixWrapper::Matrix& K_i);
106 
108 
112  virtual void CalculateCovariance( MatrixWrapper::Matrix& R_vf, MatrixWrapper::Matrix& H_i, MatrixWrapper::Matrix& invS, MatrixWrapper::Matrix& SR );
113 
114  private:
116  unsigned int nr_iterations;
118  MatrixWrapper::Matrix JP;
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 
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 SRCovarianceSet(MatrixWrapper::Matrix JP_new)
Set the square root covariance to a specific value.
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...
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 nr_iterations
Variable indicating the number of iterations of the filter.
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)


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