kfilter_impl.hpp

Go to the documentation of this file.

// This file is part of kfilter.
// kfilter is a C++ variable-dimension extended kalman filter library.
//
// Copyright (C) 2004        Vincent Zalzal, Sylvain Marleau
// Copyright (C) 2001, 2004  Richard Gourdeau
// Copyright (C) 2004        GRPR and DGE's Automation sector
//                           École Polytechnique de Montréal
//
// Code adapted from algorithms presented in :
//      Bierman, G. J. "Factorization Methods for Discrete Sequential
//      Estimation", Academic Press, 1977.
//
// This library 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 library 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 library; if not, write to the Free Software
// Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

#ifndef KFILTER_IMPL_HPP
#define KFILTER_IMPL_HPP


namespace Kalman {

  template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
  KFilter<T, BEG, OQ, OVR, DBG>::~KFilter() {}

  template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
  void KFilter<T, BEG, OQ, OVR, DBG>::makeBaseB() {}

  template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
  void KFilter<T, BEG, OQ, OVR, DBG>::makeB() {}

  template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
  void KFilter<T, BEG, OQ, OVR, DBG>::makeProcess() {

    // x = Ax + Bu + Ww    n.1 = n.n * n.1 + n.nu * nu.1
    makeB();

    K_UINT_32 i, j;
    x__.resize(n);

    for (i = BEG; i < n + BEG; ++i) {

      x__(i) = T(0.0);

      for (j = BEG; j < n + BEG; ++j)
        x__(i) += A(i,j) * x(j);

      for (j = BEG; j < nu + BEG; ++j)
        x__(i) += B(i,j) * u(j);

    }

    x.swap(x__);

  }

  template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
  void KFilter<T, BEG, OQ, OVR, DBG>::makeMeasure() {
    
    // z = Hx + Vv
    K_UINT_32 i, j;

    z.resize(m);
    for (i = BEG; i < m + BEG; ++i) {

      z(i) = T(0.0);

      for (j = BEG; j < n + BEG; ++j)
        z(i) += H(i,j) * x(j);

    }

  }

  template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
  void KFilter<T, BEG, OQ, OVR, DBG>::sizeUpdate() {

    if (flags & ( KALMAN_N_MODIFIED | KALMAN_NU_MODIFIED ) ) {
      B.resize(n, nu);
      makeBaseB();
    }   
    
    EKFilter<T, BEG, OQ, OVR, DBG>::sizeUpdate();
  }

}

#endif