Go back to Richel Bilderbeek's homepage.

Go back to Richel Bilderbeek's C++ page.

 

 

 

 

 

(C++) Kalman filter example 4: single state, use of 1x1 boost::numeric::ublas::matrix and boost::numeric::ublas::vector

 

Kalman filter example 4: single state, use of 1x1 boost::numeric::ublas::matrix and boost::numeric::ublas::vector is an example to use a Kalman filter.

 

 

 

 

 

 

Technical facts

 

Application type(s)

Operating system(s) or programming environment(s)

IDE(s):

Project type:

C++ standard:

Compiler(s):

Libraries used:

 

 

 

 

 

Qt project file: CppKalmanFilterExample4.pro

 

TEMPLATE = app
CONFIG += console
CONFIG -= qt
QMAKE_CXXFLAGS += -Wall -Wextra -Werror

win32 {
  INCLUDEPATH += E:/boost_1_50_0

  LIBS += \
    -LE:/boost_1_50_0/stage/lib  \
    #-lboost_system-mgw47-mt-d-1_50 \     #NEXT for GCC 4.7
    #-lboost_filesystem-mgw47-mt-d-1_50 \ #NEXT for GCC 4.7
    #-lboost_regex-mgw47-mt-d-1_50 \      #NEXT for GCC 4.7
    -lboost_system-mgw44-mt-1_50 \        #PREV for GCC 4.4
    -lboost_filesystem-mgw44-mt-1_50 \    #PREV for GCC 4.4
    -lboost_regex-mgw44-mt-1_50           #PREV for GCC 4.4

}


SOURCES += main.cpp \
    kalmanfilter.cpp \
    whitenoisesystem.cpp

HEADERS += \
    kalmanfilter.h \
    whitenoisesystem.h

 

 

 

 

 

kalmanfilter.h

 

#ifndef KALMANFILTER_H
#define KALMANFILTER_H

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix.hpp>

struct KalmanFilter
{
  ///Initialize the filter with a first measurent
  KalmanFilter(
    const boost::numeric::ublas::vector<double>& first_x,
    const boost::numeric::ublas::matrix<double>& first_p,
    const boost::numeric::ublas::matrix<double>& measurement_noise,
    const boost::numeric::ublas::matrix<double>& observation,
    const boost::numeric::ublas::matrix<double>& process_noise,
    const boost::numeric::ublas::matrix<double>& state_transition
    );

  ///Give the filter a real measurement, so it will update itself
  void SupplyMeasurement(const boost::numeric::ublas::vector<double>& x);

  ///Let the filter predict
  const boost::numeric::ublas::vector<double>& Predict() const { return m_x; }

  ///Let the filter predict
  const boost::numeric::ublas::matrix<double>& PredictCovariance() const { return m_p; }

  private:

  //R: Estimated measurement noise: How to estimate this?
  const boost::numeric::ublas::matrix<double> m_measurement_noise;

  //H
  const boost::numeric::ublas::matrix<double> m_observation;

  ///The (current prediction of the) covariance
  boost::numeric::ublas::matrix<double> m_p;

  //Q: Process noise: How to estimate this?
  const boost::numeric::ublas::matrix<double> m_process_noise;

  //F: state transition matrix
  const boost::numeric::ublas::matrix<double> m_state_transition;

  ///The (current prediction of the) measurement
  boost::numeric::ublas::vector<double> m_x;

};

#endif // KALMANFILTER_H

 

 

 

 

 

kalmanfilter.cpp

 

#include "kalmanfilter.h"

KalmanFilter::KalmanFilter(
  const boost::numeric::ublas::vector<double>& first_x,
  const boost::numeric::ublas::matrix<double>& first_p,
  const boost::numeric::ublas::matrix<double>& measurement_noise,
  const boost::numeric::ublas::matrix<double>& observation,
  const boost::numeric::ublas::matrix<double>& process_noise,
  const boost::numeric::ublas::matrix<double>& state_transition)
  : m_measurement_noise(measurement_noise),
    m_observation(observation),
    m_p(first_p),
    m_process_noise(process_noise),
    m_state_transition(state_transition),
    m_x(first_x)
{
  assert(process_noise.size1() >= 1);
  assert(process_noise.size2() >= 1);
}

const boost::numeric::ublas::matrix<double> Inverse(
  const boost::numeric::ublas::matrix<double>& m)
{
  assert(m.size1() == m.size2() && "Can only calculate the inverse of square matrices");
  assert(m.size1() == 1 && m.size2() == 1 && "Only for 1x1 matrices");
  assert(m(0,0) != 0.0 && "Cannot take the inverse of matrix [0]");
  boost::numeric::ublas::matrix<double> n(1,1);
  n(0,0) =  1.0 / m(0,0);
  return n;
}

void KalmanFilter::SupplyMeasurement(const boost::numeric::ublas::vector<double>& x)
{
  using boost::numeric::ublas::identity_matrix;
  using boost::numeric::ublas::matrix;
  using boost::numeric::ublas::prod;
  using boost::numeric::ublas::trans;
  using boost::numeric::ublas::vector;
  ///Debug statements to keep code below clean
  /// 2/7) Covariance prediction
  assert(m_state_transition.size2() == m_p.size1());
  assert( prod(m_state_transition,m_p).size2()
    ==  trans(m_state_transition).size1() );
  assert(matrix<double>(prod(
      matrix<double>(prod(m_state_transition,m_p)),
      trans(m_state_transition))).size1()
    == m_process_noise.size1());
  assert(matrix<double>(prod(
      matrix<double>(prod(m_state_transition,m_p)),
      trans(m_state_transition))).size2()
    == m_process_noise.size2());

  /// 1/7) State prediction
  const vector<double> x_current = prod(m_state_transition,m_x);
  /// 2/7) Covariance prediction
  const matrix<double> p_current
    = matrix<double>(
      prod(
        matrix<double>(prod(m_state_transition,m_p)),
        trans(m_state_transition)
      )
    )
    + m_process_noise;
  /// 3/7) Innovation (y with a squiggle above it)
  const vector<double> z_measured = x; //x has noise in it
  const vector<double> innovation = z_measured - prod(m_observation,x_current);
  /// 4/7) Innovation covariance (S)
  const matrix<double> innovation_covariance
    = matrix<double>(prod(
          matrix<double>(prod(m_observation,p_current)),
          trans(m_observation)
        ))
    + m_measurement_noise;
  /// 5/7) Kalman gain (K)
  const matrix<double> kalman_gain
    = prod(
        matrix<double>(
        prod(
          p_current,
          trans(m_observation)
        )),
        Inverse(innovation_covariance)
      );
  /// 6/7) Update state prediction
  m_x = x_current + prod(kalman_gain,innovation);
  /// 7/7) Update covariance prediction
  const identity_matrix<double> my_identity_matrix(kalman_gain.size1());
  m_p = prod(
    my_identity_matrix
      - matrix<double>(prod(kalman_gain,m_observation)),
    p_current
  );
}

 

 

 

 

 

main.cpp

 

///Kalman filter example
///Adapted from merge from www.adrianboeing.com and http://greg.czerniak.info/guides/kalman1
///now working with matrices

#include <iostream>
#include <boost/shared_ptr.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_expression.hpp>
#include "kalmanfilter.h"
#include "whitenoisesystem.h"

///Context:
///A constant DC voltage measured with a noisy multimeter
int main()
{
  using boost::numeric::ublas::matrix;
  using boost::numeric::ublas::vector;

  ///The state matrix
  const vector<double> x_real(1,1.25); //Volts

  ///Real measurement noise
  const vector<double> x_real_noise(1,0.25);

  ///Guess of the state matrix
  const vector<double> x_first_guess(1,10.0);

  ///Guess of the covariances
  const matrix<double> p_first_guess(1,1,1.0);

  //Estimated measurement noise
  const matrix<double> measurement_noise(1,1,0.1);

  const matrix<double> observation(1,1,1.0);

  //Estimated process noise
  const matrix<double> process_noise(1,1,0.0001);

  const matrix<double> state_transition(1,1,1.0);

  const WhiteNoiseSystem s(x_real,x_real_noise);

  KalmanFilter k(x_first_guess,p_first_guess,measurement_noise,observation,process_noise,state_transition);
\
  std::cout << "Real,measured,Kalman\n";
  for (int i=0;i!=100;++i)
  {
    //Perform a noisy measurement
    const vector<double> z_measured = s.Measure();
    //Pass this measurement to the filter
    k.SupplyMeasurement(z_measured);
    //Display what the filter predicts
    const vector<double> x_est_last = k.Predict();
    std::cout << x_real(0) << "," << z_measured(0) << "," << x_est_last(0) << '\n';
  }
}

 

 

 

 

 

whitenoisesystem.h

 

#ifndef WHITENOISESYSTEM_H
#define WHITENOISESYSTEM_H

#include <boost/numeric/ublas/matrix.hpp>

struct WhiteNoiseSystem
{
  WhiteNoiseSystem(
    const boost::numeric::ublas::vector<double>& real_value,
    const boost::numeric::ublas::vector<double>& stddev);

  ///Measure a value from this system with normally distributed noise
  const boost::numeric::ublas::vector<double> Measure() const;

  ///Peek what the real value is
  const boost::numeric::ublas::vector<double>& PeekAtRealValue() const { return m_mean; }

  private:
  ///The real value of the system
  const boost::numeric::ublas::vector<double> m_mean;

  ///The amount of noise in the system
  ///A noise of zero indicates a system that can be measured accurately to infinite precision
  const boost::numeric::ublas::vector<double> m_stddev;

  ///Obtain a random number from a normal distribution
  ///From http://www.richelbilderbeek.nl/CppGetRandomNormal.htm
  static double GetRandomNormal(const double mean = 0.0, const double sigma = 1.0);

};

#endif // WHITENOISESYSTEM_H

 

 

 

 

 

whitenoisesystem.cpp

 

#include "whitenoisesystem.h"

#include <boost/random/normal_distribution.hpp>
#include <boost/random/lagged_fibonacci.hpp>

WhiteNoiseSystem::WhiteNoiseSystem(
  const boost::numeric::ublas::vector<double>& real_value,
  const boost::numeric::ublas::vector<double>& stddev)
  : m_mean(real_value),
    m_stddev(stddev)
{
  assert(m_mean.size() == m_stddev.size() && "Every value must have one measurement noise");
}

double WhiteNoiseSystem::GetRandomNormal(const double mean, const double sigma)
{
  boost::normal_distribution<double> norm_dist(mean, sigma);
  static boost::lagged_fibonacci19937 engine;
  const double value = norm_dist.operator () <boost::lagged_fibonacci19937>((engine));
  return value;
}

const boost::numeric::ublas::vector<double> WhiteNoiseSystem::Measure() const
{
  const std::size_t sz = m_mean.size();
  boost::numeric::ublas::vector<double> measured(sz);
  for (std::size_t i = 0; i!=sz; ++i)
  {
    measured(i) = GetRandomNormal(m_mean(i),m_stddev(i));
  }
  return measured;
}

 

 

 

 

 

Go back to Richel Bilderbeek's C++ page.

Go back to Richel Bilderbeek's homepage.

 

Valid XHTML 1.0 Strict

This page has been created by the tool CodeToHtml