SRiteratedextendedkalmanfilter.h

Go to the documentation of this file.

// $Id$
// Copyright (C) 2002 Klaas Gadeyne <first dot last at gmail dot com>
//                    Peter Slaets  <peter dot slaets at mech dot kuleuven dot ac dot be>
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation; either version 2.1 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
//

#ifndef __SR_ITERATED_EXTENDED_KALMAN_FILTER__
#define __SR_ITERATED_EXTENDED_KALMAN_FILTER__

#include "kalmanfilter.h"
#include "../pdf/conditionalpdf.h"
#include "../pdf/gaussian.h"

namespace BFL
{

  class SRIteratedExtendedKalmanFilter : public KalmanFilter
    {
    public:
      SRIteratedExtendedKalmanFilter(Gaussian* prior, unsigned int nr_it=1);

      virtual ~SRIteratedExtendedKalmanFilter();
      virtual void SysUpdate(SystemModel<MatrixWrapper::ColumnVector>* const sysmodel,
                             const MatrixWrapper::ColumnVector& u);
      virtual void SysUpdate(SystemModel<MatrixWrapper::ColumnVector>* const sysmodel);
      virtual void MeasUpdate(MeasurementModel<MatrixWrapper::ColumnVector,MatrixWrapper::ColumnVector>* const measmodel,
                              const MatrixWrapper::ColumnVector& z,
                              const MatrixWrapper::ColumnVector& s);
      virtual void MeasUpdate(MeasurementModel<MatrixWrapper::ColumnVector,MatrixWrapper::ColumnVector>* const measmodel,const MatrixWrapper::ColumnVector& z);
      MatrixWrapper::Matrix SRCovarianceGet() const;
      void      SRCovarianceSet( MatrixWrapper::Matrix JP_new);
      void      PriorSet(MatrixWrapper::ColumnVector& X, MatrixWrapper::SymmetricMatrix& P);

      virtual void CalculateMeasUpdate(MatrixWrapper::ColumnVector z, MatrixWrapper::ColumnVector Z, MatrixWrapper::Matrix H, MatrixWrapper::SymmetricMatrix R);


      virtual void  CalculateMatrix(MatrixWrapper::Matrix& H_i  , MatrixWrapper::SymmetricMatrix& R_i , MatrixWrapper::Matrix& invS  , MatrixWrapper::Matrix& Sr  , MatrixWrapper::Matrix& K_i );


      virtual void CalculateMean(MatrixWrapper::ColumnVector& x_k, const MatrixWrapper::ColumnVector& z, MatrixWrapper::ColumnVector& Z_i ,MatrixWrapper::Matrix& K_i);


      virtual void CalculateCovariance( MatrixWrapper::Matrix& R_vf, MatrixWrapper::Matrix& H_i, MatrixWrapper::Matrix& invS, MatrixWrapper::Matrix& SR );

    private:
      unsigned int nr_iterations;
      MatrixWrapper::Matrix JP;
      //BFL::Matrix invS;
      //BFL::Matrix Sr;
      //BFL::Matrix K_i;
    };  // class

} // End namespace BFL

#endif // __ITERATED_EXTENDED_KALMAN_FILTER__