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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 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
27namespace BFL
28{
29
54 {
55 public:
61 SRIteratedExtendedKalmanFilter(Gaussian* prior, unsigned int nr_it=1);
62
67 const MatrixWrapper::ColumnVector& u);
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
Class representing Gaussian (or normal density)
Definition gaussian.h:28
Class representing the family of all Kalman Filters (EKF, IEKF, ...)
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)
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.
virtual void CalculateMean(MatrixWrapper::ColumnVector &x_k, const MatrixWrapper::ColumnVector &z, MatrixWrapper::ColumnVector &Z_i, MatrixWrapper::Matrix &K_i)
Calculate the new state estimate.
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.
void PriorSet(MatrixWrapper::ColumnVector &X, MatrixWrapper::SymmetricMatrix &P)
Set mean and covariance of the state estimation to a specific value.
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.
virtual ~SRIteratedExtendedKalmanFilter()
Destructor.
virtual void SysUpdate(SystemModel< MatrixWrapper::ColumnVector > *const sysmodel)
Perform a system update with the current measurement model and system model.
void SRCovarianceSet(MatrixWrapper::Matrix JP_new)
Set the square root covariance to a specific value.
virtual void MeasUpdate(MeasurementModel< MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector > *const measmodel, const MatrixWrapper::ColumnVector &z)
Perform a measurement update use a measurement model, measurements z.