itkKalmanLinearEstimator.h

Go to the documentation of this file.

/*=========================================================================
 *
 *  Copyright Insight Software Consortium
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *         http://www.apache.org/licenses/LICENSE-2.0.txt
 *
 *  Unless required by applicable law or agreed to in writing, software
 *  distributed under the License is distributed on an "AS IS" BASIS,
 *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *  See the License for the specific language governing permissions and
 *  limitations under the License.
 *
 *=========================================================================*/
#ifndef itkKalmanLinearEstimator_h
#define itkKalmanLinearEstimator_h

#include "itkMacro.h"

#include "vnl/vnl_vector_fixed.h"
#include "vnl/vnl_matrix_fixed.h"

namespace itk
{
template< typename T, unsigned int VEstimatorDimension >
class KalmanLinearEstimator
{
public:
  static constexpr unsigned int Dimension = VEstimatorDimension;

  using VectorType = vnl_vector_fixed< T, VEstimatorDimension >;

  using MatrixType = vnl_matrix_fixed< T, VEstimatorDimension, VEstimatorDimension >;

  using ValueType = T;

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

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

  void ClearVariance()
  {
    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() const
  { return m_Estimator; }

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

private:
  void UpdateVariance(const VectorType  &);

  VectorType m_Estimator;

  MatrixType m_Variance;
};

template< typename 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< typename 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