itkKalmanLinearEstimator.h

Go to the documentation of this file.

/*=========================================================================
 *
 *  Copyright NumFOCUS
 *
 *  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
 *
 *         https://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)
{
  const ValueType measurePrediction = dot_product(newPredictor, m_Estimator);
 
  const 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;
 
  const 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