Go back to Richel Bilderbeek's homepage.

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

 

 

 

 

 

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

 

Kalman filter example 3: single state, use of 1x1 boost::numeric::ublas::matrix 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: CppKalmanFilterExample3.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::matrix<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>& process_noise
    );

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

  ///Let the filter predict
  const boost::numeric::ublas::matrix<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;

  ///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;

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

};

#endif // KALMANFILTER_H

 

 

 

 

 

kalmanfilter.cpp

 

#include "kalmanfilter.h"



KalmanFilter::KalmanFilter(
  const boost::numeric::ublas::matrix<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>& process_noise)
  : m_measurement_noise(measurement_noise),
    m_p(first_p),
    m_process_noise(process_noise),
    m_x(first_x)
{

}

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::matrix<double>& x)
{
  using boost::numeric::ublas::identity_matrix;
  using boost::numeric::ublas::matrix;
  using boost::numeric::ublas::prod;

  /// 1/7) State prediction
  const matrix<double> x_current = m_x;
  /// 2/7) Covariance prediction
  const matrix<double> p_current = m_p + m_process_noise;
  /// 3/7) Innovation (y with a squiggle above it)
  const matrix<double> z_measured = x; //x has noise in it
  const matrix<double> innovation = z_measured - x_current;
  /// 4/7) Innovation covariance (S)
  const matrix<double> innovation_covariance = p_current + m_measurement_noise;
  /// 5/7) Kalman gain (K)
  const matrix<double> kalman_gain
    = prod(p_current,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 - kalman_gain,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.hpp>
#include "kalmanfilter.h"
#include "whitenoisesystem.h"

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

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

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

  ///Guess of the state matrix
  const matrix<double> x_first_guess(1,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);

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

  const WhiteNoiseSystem s(x_real,x_real_noise);

  KalmanFilter k(x_first_guess,p_first_guess,measurement_noise,process_noise);

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

 

 

 

 

 

whitenoisesystem.h

 

#ifndef WHITENOISESYSTEM_H
#define WHITENOISESYSTEM_H

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

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

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

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

  private:
  ///The real value of the system
  const boost::numeric::ublas::matrix<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::matrix<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::matrix<double>& real_value,
  const boost::numeric::ublas::matrix<double>& stddev)
  : m_mean(real_value),
    m_stddev(stddev)
{
  assert(m_mean.size1() == m_stddev.size1() && "Every value must have one measurement noise");
  assert(m_mean.size2() == m_stddev.size2() && "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::matrix<double> WhiteNoiseSystem::Measure() const
{
  const std::size_t n_rows = m_mean.size1();
  const std::size_t n_cols = m_mean.size2();
  boost::numeric::ublas::matrix<double> measured(n_rows,n_cols);
  for (std::size_t row = 0; row!=n_rows; ++row)
  {
    for (std::size_t col = 0; col!=n_cols; ++col)
    {
      measured(row,col) = GetRandomNormal(m_mean(row,col),m_stddev(row,col));
    }
  }
  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