itkKalmanLinearEstimator.h

Go to the documentation of this file.

/*=========================================================================

  Program:   Insight Segmentation & Registration Toolkit
  Module:    $RCSfile: itkKalmanLinearEstimator.h,v $
  Language:  C++
  Date:      $Date: 2009-01-24 20:02:57 $
  Version:   $Revision: 1.17 $

  Copyright (c) Insight Software Consortium. All rights reserved.
  See ITKCopyright.txt or http://www.itk.org/HTML/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even 
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR 
     PURPOSE.  See the above copyright notices for more information.

=========================================================================*/
#ifndef __itkKalmanLinearEstimator_h
#define __itkKalmanLinearEstimator_h

#include "itkMacro.h"

#include <vnl/vnl_vector_fixed.h>
#include <vnl/vnl_matrix_fixed.h>

namespace itk
{

template <class T, unsigned int VEstimatorDimension>
class ITK_EXPORT KalmanLinearEstimator 
{
public:
  itkStaticConstMacro( Dimension, unsigned int,
                       VEstimatorDimension);

  typedef vnl_vector_fixed<T,VEstimatorDimension> VectorType;

  typedef vnl_matrix_fixed<T,VEstimatorDimension,VEstimatorDimension> MatrixType;

  typedef T ValueType;

  void UpdateWithNewMeasure(  const ValueType & newMeasure,
                              const VectorType & newPredictor );

  void ClearEstimation(void) 
    { m_Estimator = VectorType(T(0)); }

  void ClearVariance(void)
    {
    m_Variance.set_identity();
    }

  void SetVariance(const ValueType & var = 1.0) 
    {
    m_Variance.set_identity();
    m_Variance *= var;
    }

  void SetVariance(const MatrixType & m)
    { m_Variance = m; }

  const VectorType & GetEstimator(void) const
    { return m_Estimator; }

  const MatrixType & GetVariance(void) const
    { return m_Variance; }

private:  
  void UpdateVariance( const VectorType  & );

  VectorType  m_Estimator;

  MatrixType m_Variance;

};


template <class T, unsigned int VEstimatorDimension>
void
KalmanLinearEstimator<T,VEstimatorDimension>
::UpdateWithNewMeasure( const ValueType  & newMeasure, 
                        const VectorType & newPredictor   )
{
  ValueType measurePrediction = dot_product( newPredictor , m_Estimator );
  
  ValueType errorMeasurePrediction = newMeasure - measurePrediction;

  VectorType Corrector = m_Variance * newPredictor;

  for( unsigned int j=0; j<VEstimatorDimension; j++) 
    {
    m_Estimator(j) += Corrector(j) * errorMeasurePrediction;
    }
  
  UpdateVariance( newPredictor );

}


template <class T, unsigned int VEstimatorDimension>
void
KalmanLinearEstimator<T,VEstimatorDimension>
::UpdateVariance( const VectorType & newPredictor )
{  
  VectorType aux =  m_Variance * newPredictor;

  ValueType denominator = 1.0/(1.0 +  dot_product( aux , newPredictor ) );

  for( unsigned int col=0; col<VEstimatorDimension; col++) 
    {
    for( unsigned int row=0; row<VEstimatorDimension; row++) 
      {
      m_Variance(col,row) -= aux(col)*aux(row)*denominator;
      }
    }
}

} // end namespace itk

#endif