Go back to Richel Bilderbeek's homepage.

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

 

 

 

 

 

(C++) KalmanFilterExample6

 

Kalman filter example 6 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: CppKalmanFilterExample6Console.pro

 

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

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 \
    matrix.cpp \
    maindialog.cpp

HEADERS += \
    kalmanfilter.h \
    whitenoisesystem.h \
    matrix.h \
    maindialog.h

 

 

 

 

 

Qt project file: CppKalmanFilterExample6Desktop.pro

 

QT       += core gui qwt
TEMPLATE = app
QMAKE_CXXFLAGS += -Wall -Wextra -Werror -std=c++11

unix {
  INCLUDEPATH += /usr/include/qwt-qt4
  LIBS += -lqwt-qt4
}

win32 {
  INCLUDEPATH += E:/boost_1_50_0 C:/qwt-6.0.1/include
  LIBS += \
    -LE:/boost_1_50_0/stage/lib  \
    -lboost_system-mgw44-mt-1_50 \
    -lboost_filesystem-mgw44-mt-1_50 \
    -lboost_regex-mgw44-mt-1_50 \
    -LC:/qwt-6.0.1/lib \
    -lqwtd  #Note: gives error 'QWidget: Must construct a QApplication before a QPaintDevice' when using '-lqwt'
    #-lQtSvg
}


SOURCES += \
    qtmain.cpp \
    qtmaindialog.cpp \
    kalmanfilter.cpp \
    whitenoisesystem.cpp \
    matrix.cpp \
    maindialog.cpp

HEADERS  += \
    qtmaindialog.h \
    whitenoisesystem.h \
    matrix.h \
    kalmanfilter.h \
    maindialog.h

FORMS    += qtmaindialog.ui

 

 

 

 

 

kalmanfilter.h

 

#ifndef KALMANFILTER_H
#define KALMANFILTER_H

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

struct KalmanFilter
{
  ///Initialize the filter with a first measurent
  KalmanFilter(
    const boost::numeric::ublas::matrix<double>& control,
    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_covariance,
    const boost::numeric::ublas::matrix<double>& state_transition
    );

  ///Give the filter a measurement (but no input), and it will update its predictions.
  void SupplyMeasurement(const boost::numeric::ublas::vector<double>& x);

  ///Give the filter a measurement and input, and it will update its predictions
  void SupplyMeasurementAndInput(
    const boost::numeric::ublas::vector<double>& x,
    const boost::numeric::ublas::vector<double>& input);

  ///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:
  //B: control matrix: the effect of inputs on the current states
  const boost::numeric::ublas::matrix<double> m_control;

  //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 covariance: How to estimate this?
  const boost::numeric::ublas::matrix<double> m_process_noise_covariance;

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

 

#ifdef _WIN32
#undef __STRICT_ANSI__
#endif

#include "kalmanfilter.h"
#include "matrix.h"

KalmanFilter::KalmanFilter(
  const boost::numeric::ublas::matrix<double>& control,
  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_covariance,
  const boost::numeric::ublas::matrix<double>& state_transition)
  : m_control(control),
    m_measurement_noise(measurement_noise),
    m_observation(observation),
    m_p(first_p),
    m_process_noise_covariance(process_noise_covariance),
    m_state_transition(state_transition),
    m_x(first_x)
{
  #ifndef NDEBUG
  //Check for correct dimensionality
  const auto sz = m_x.size();
  assert(m_control.size1() == sz);
  assert(m_control.size2() == sz);
  assert(m_measurement_noise.size1() == sz);
  assert(m_measurement_noise.size2() == sz);
  assert(m_observation.size1() == sz);
  assert(m_observation.size2() == sz);
  assert(m_p.size1() == sz);
  assert(m_p.size2() == sz);
  assert(m_process_noise_covariance.size1() == sz);
  assert(m_process_noise_covariance.size2() == sz);
  assert(m_state_transition.size1() == sz);
  assert(m_state_transition.size2() == sz);
  assert(m_x.size() == sz);
  #endif
}

void KalmanFilter::SupplyMeasurement(
  const boost::numeric::ublas::vector<double>& x)
{
  const boost::numeric::ublas::vector<double> input
    = boost::numeric::ublas::vector<double>(x.size(),0.0);
  return SupplyMeasurementAndInput(x,input);
}

void KalmanFilter::SupplyMeasurementAndInput(
  const boost::numeric::ublas::vector<double>& x,
  const boost::numeric::ublas::vector<double>& input)
{
  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_covariance.size1());
  assert(matrix<double>(prod(
      matrix<double>(prod(m_state_transition,m_p)),
      trans(m_state_transition))).size2()
    == m_process_noise_covariance.size2());

  /// 1/7) State prediction
  const vector<double> x_current
    = prod(m_state_transition,m_x)
    + prod(m_control,input);
  /// 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_covariance;
  /// 3/7) Innovation (y with a squiggle above it)
  const vector<double> z_measured = x; //x has noise itn 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)
        )),
        Matrix::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

 

#ifdef _WIN32
#undef __STRICT_ANSI__
#endif

///Kalman filter example
///Adapted from merge from www.adrianboeing.com and http://greg.czerniak.info/guides/kalman1
///following
/// * Simon, Dan. Kalman Filtering. Embedded Systems Programming. June 2001.

#include <iostream>
#include "maindialog.h"
#include "matrix.h"

///Context:
///A car that has a constant acceleration that has its position determined by GPS
///The car its speedometer is not used (as observation(1,1) is equal to 0.0),
///  and gives junk values (as x_real_noise(1) is equal to 10000000.0)
int main()
{
  const double t = 0.1;
  const double acceleration = 1.0;

  //The real state vector
  //[ position ]
  //[ velocity ]
  const boost::numeric::ublas::vector<double> init_x_real = Matrix::CreateVector( { 0.0, 0.0 } );

  //Real measurement noise
  //[ standard deviation of noise in position ]   [ standard deviation of noise in GPS                       ]
  //[ standard deviation of noise in velocity ] = [ standard deviation of noise in defect/unused speedometer ]
  const boost::numeric::ublas::vector<double> x_real_measurement_noise = Matrix::CreateVector( { 10.0, 10000000.0 } );

  //Guess of the state matrix
  //Position and velocity guess is way off on purpose
  //[ position ]
  //[ velocity ]
  const boost::numeric::ublas::vector<double> x_first_guess = Matrix::CreateVector( { 100.0, 10.0 } );

  //Guess of the covariances
  //[ 1.0   0.0 ]
  //[ 0.0   1.0 ]
  const boost::numeric::ublas::matrix<double> p_first_guess = Matrix::CreateMatrix(2,2, { 1.0, 0.0, 0.0, 1.0 } );

  //Effect of inputs on state
  //Input = gas pedal, which gives acceleration 'a'
  //[ 1.0   0.5 * t * t ]   [teleportation (not used)                 x = 0.5 * a * t * t ]
  //[ 0.0   t           ] = [no effect of teleportation on velocity   v = a * t           ]
  const boost::numeric::ublas::matrix<double> control = Matrix::CreateMatrix(2,2, { 1.0, 0.0, 0.5 * t * t, t } );

  //Estimated measurement noise
  //[ 10.0          0.0 ]   [ Estimated noise in GPS   ?                                                     ]
  //[  0.0   10000000.0 ] = [ ?                        Estimated noise in speedometer (absent in this setup) ]
  const boost::numeric::ublas::matrix<double> measurement_noise = Matrix::CreateMatrix(2,2, { 10.0, 0.0, 0.0, 10000000.0 } );

  //Observational matrix
  //[ 1.0   0.0 ]   [GPS measurement   ?                                         ]
  //[ 0.0   0.0 ] = [?                 Speedometer (absent/unused in this setup) ]
  const boost::numeric::ublas::matrix<double> observation = Matrix::CreateMatrix(2,2, { 1.0, 0.0, 0.0, 0.0 } );

  //Real process noise
  //[ 0.001 ]   [ noise in position ]
  //[ 0.001 ] = [ noise in velocity ]
  const boost::numeric::ublas::vector<double> real_process_noise = Matrix::CreateVector( {0.01,0.01} );

  //Estimated process noise covariance
  //[ 0.01   0.01 ]
  //[ 0.01   0.01 ]
  const boost::numeric::ublas::matrix<double> process_noise = Matrix::CreateMatrix(2,2,{0.01,0.01,0.01,0.01});

  //State transition matrix, the effect of the current state on the next
  //[ 1.0     t ]   [ position keeps its value             a velocity changes the position ]
  //[ 0.0   1.0 ] = [ position has no effect on velocity   a velocity keeps its value      ]
  const boost::numeric::ublas::matrix<double> state_transition = Matrix::CreateMatrix(2,2,{1.0,0.0,t,1.0});

  const int time = 250;
  const MainDialog d(
    time,
    acceleration,
    control,
    measurement_noise,
    observation,
    p_first_guess,
    process_noise,
    state_transition,
    init_x_real,
    real_process_noise,
    x_first_guess,
    x_real_measurement_noise);

  //Display header
  {
    const boost::numeric::ublas::vector<std::string> header = d.GetHeader();
    const std::size_t sz = header.size();
    for (std::size_t i=0; i!=sz; ++i)
    {
      std::cout << header(i);
      if (i != sz - 1) std::cout << ",";
    }
    std::cout << '\n';
  }
  //Display data
  {
    const boost::numeric::ublas::matrix<double>& data = d.GetData();
    const std::size_t n_cols = 6;
    assert(n_cols == d.GetHeader().size());
    for (int row=0; row!=time; ++row)
    {
      for (std::size_t col=0; col!=n_cols; ++col)
      {
        std::cout << data(row,col);
        if (col != 5) std::cout << ",";
      }
      std::cout << '\n';
    }
  }
}

 

 

 

 

 

maindialog.h

 

#ifndef MAINDIALOG_H
#define MAINDIALOG_H

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

struct MainDialog
{
  MainDialog(
    const int time,
    const double acceleration,
    const boost::numeric::ublas::matrix<double>& control,
    const boost::numeric::ublas::matrix<double>& measurement_noise,
    const boost::numeric::ublas::matrix<double>& observation,
    const boost::numeric::ublas::matrix<double>& p_first_guess,
    const boost::numeric::ublas::matrix<double>& process_noise,
    const boost::numeric::ublas::matrix<double>& state_transition,
    const boost::numeric::ublas::vector<double>& init_x_real,
    const boost::numeric::ublas::vector<double>& real_process_noise,
    const boost::numeric::ublas::vector<double>& x_first_guess,
    const boost::numeric::ublas::vector<double>& x_real_measurement_noise);


  ///Obtain the simulation data
  const boost::numeric::ublas::matrix<double>& GetData() const { return m_data; }

  ///Obtain the header text
  static const boost::numeric::ublas::vector<std::string> GetHeader();

  private:

  ///The simulation data
  const boost::numeric::ublas::matrix<double> m_data;

  //Real initial state
  const boost::numeric::ublas::vector<double> m_init_x_real;

  //Real measurement noise
  const boost::numeric::ublas::vector<double> m_x_real_measurement_noise;

  //Guess of the state matrix
  const boost::numeric::ublas::vector<double> m_x_first_guess;

  //Guess of the covariances
  const boost::numeric::ublas::matrix<double> m_p_first_guess;

  //Effect of inputs on state
  const boost::numeric::ublas::matrix<double> m_control;

  //Estimated measurement noise
  const boost::numeric::ublas::matrix<double> m_measurement_noise;

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

  //Real process noise
  const boost::numeric::ublas::vector<double> m_real_process_noise;

  //Estimated process noise covariance
  const boost::numeric::ublas::matrix<double> m_process_noise;

  //State transition matrix, the effect of the current state on the next
  const boost::numeric::ublas::matrix<double> m_state_transition;


  ///Create the simulation data
  static const boost::numeric::ublas::matrix<double> CreateData(
    const int time,
    const double acceleration,
    const boost::numeric::ublas::matrix<double>& control,
    const boost::numeric::ublas::matrix<double>& measurement_noise,
    const boost::numeric::ublas::matrix<double>& observation,
    const boost::numeric::ublas::matrix<double>& p_first_guess,
    const boost::numeric::ublas::matrix<double>& process_noise,
    const boost::numeric::ublas::matrix<double>& state_transition,
    const boost::numeric::ublas::vector<double>& init_x_real,
    const boost::numeric::ublas::vector<double>& real_process_noise,
    const boost::numeric::ublas::vector<double>& x_first_guess,
    const boost::numeric::ublas::vector<double>& x_real_measurement_noise);
};

#endif // MAINDIALOG_H

 

 

 

 

 

maindialog.cpp

 

#ifdef _WIN32
#undef __STRICT_ANSI__
#endif

#include "maindialog.h"

#include <vector>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include "kalmanfilter.h"
#include "matrix.h"
#include "whitenoisesystem.h"

MainDialog::MainDialog(
  const int time,
  const double acceleration,
  const boost::numeric::ublas::matrix<double>& control,
  const boost::numeric::ublas::matrix<double>& measurement_noise,
  const boost::numeric::ublas::matrix<double>& observation,
  const boost::numeric::ublas::matrix<double>& p_first_guess,
  const boost::numeric::ublas::matrix<double>& process_noise,
  const boost::numeric::ublas::matrix<double>& state_transition,
  const boost::numeric::ublas::vector<double>& init_x_real,
  const boost::numeric::ublas::vector<double>& real_process_noise,
  const boost::numeric::ublas::vector<double>& x_first_guess,
  const boost::numeric::ublas::vector<double>& x_real_measurement_noise)
  : m_data(
    CreateData(
      time,
      acceleration,
      control,
      measurement_noise,
      observation,
      p_first_guess,
      process_noise,
      state_transition,
      init_x_real,
      real_process_noise,
      x_first_guess,
      x_real_measurement_noise
      )
    )
{



}

const boost::numeric::ublas::matrix<double> MainDialog::CreateData(
  const int time,
  const double acceleration,
  const boost::numeric::ublas::matrix<double>& control,
  const boost::numeric::ublas::matrix<double>& measurement_noise,
  const boost::numeric::ublas::matrix<double>& observation,
  const boost::numeric::ublas::matrix<double>& p_first_guess,
  const boost::numeric::ublas::matrix<double>& process_noise,
  const boost::numeric::ublas::matrix<double>& state_transition,
  const boost::numeric::ublas::vector<double>& init_x_real,
  const boost::numeric::ublas::vector<double>& real_process_noise,
  const boost::numeric::ublas::vector<double>& x_first_guess,
  const boost::numeric::ublas::vector<double>& x_real_measurement_noise)

{
  Matrix::Test();

  using boost::numeric::ublas::matrix;
  using boost::numeric::ublas::vector;

  //The resulting matrix, has time rows and 6 columns
  matrix<double> data(time,6);
  assert(time == static_cast<int>(data.size1()));
  assert(data.size2() == 6);
  assert(GetHeader().size() == data.size2());

  WhiteNoiseSystem s(control,init_x_real,x_real_measurement_noise,real_process_noise,state_transition);

  KalmanFilter k(control,x_first_guess,p_first_guess,measurement_noise,observation,process_noise,state_transition);

  //std::cout << "x_real,x_measured,x_Kalman,v_real,v_measured,v_Kalman\n";
  for (int i=0;i!=time;++i)
  {
    //A constant push the gas pedal, which results in a constant acceleration
    const vector<double> input = Matrix::CreateVector( { 0.0, acceleration } );
    //Update reality, that is, let the real system (i.e. reality) go to its next state
    s.GoToNextState(input);
    //Perform a noisy measurement
    const vector<double> z_measured = s.Measure();
    //Pass this measurement to the filter
    k.SupplyMeasurementAndInput(z_measured,input);
    //Display what the filter predicts
    const vector<double> x_est_last = k.Predict();

    assert(i < static_cast<int>(data.size1()));
    assert(5 < data.size2());
    data(i,0) = s.PeekAtRealState()(0);
    data(i,1) = z_measured(0);
    data(i,2) = x_est_last(0);
    data(i,3) = s.PeekAtRealState()(1);
    data(i,4) = z_measured(1);
    data(i,5) = x_est_last(1);
  }
  return data;
}

const boost::numeric::ublas::vector<std::string> MainDialog::GetHeader()
{
  boost::numeric::ublas::vector<std::string> v(6);
  assert(5 < v.size());
  v(0) = "x_real";
  v(1) = "x_measured";
  v(2) = "x_Kalman";
  v(3) = "v_real";
  v(4) = "v_measured";
  v(5) = "v_Kalman";
  return v;
}

 

 

 

 

 

matrix.h

 

#ifndef MATRIX_H
#define MATRIX_H

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

///Helper class for matrix operations
struct Matrix
{
  ///Create a n_rows x n_cols sized matrix from a std::vector,
  ///used for easy initialization
  static const boost::numeric::ublas::matrix<double> CreateMatrix(
    const std::size_t n_rows,
    const std::size_t n_cols,
    const std::vector<double>& v);


  ///Create a uBLAS vector from a std::vector,
  ///used for easy initialization
  static const boost::numeric::ublas::vector<double> CreateVector(const std::vector<double>& v);

  ///Calculate the inverse of a matrix
  static const boost::numeric::ublas::matrix<double> Inverse(
    const boost::numeric::ublas::matrix<double>& m);

  ///Test these functions
  static void Test();

};

#endif // MATRIX_H

 

 

 

 

 

matrix.cpp

 

#ifdef _WIN32
#undef __STRICT_ANSI__
#endif

#include "matrix.h"

#include <iostream>
#include <boost/numeric/ublas/io.hpp>

const boost::numeric::ublas::matrix<double> Matrix::CreateMatrix(
  const std::size_t n_rows,
  const std::size_t n_cols,
  const std::vector<double>& v)
{
  assert(n_rows * n_cols == v.size());
  boost::numeric::ublas::matrix<double> m(n_rows,n_cols);
  for (std::size_t row = 0; row!=n_rows; ++row)
  {
    for (std::size_t col = 0; col!=n_cols; ++col)
    {
      m(row,col) = v[ (col * n_rows) + row];
    }
  }
  return m;
}

const boost::numeric::ublas::vector<double> Matrix::CreateVector(const std::vector<double>& v)
{
  boost::numeric::ublas::vector<double> w(v.size());
  std::copy(v.begin(),v.end(),w.begin());
  return w;
}

const boost::numeric::ublas::matrix<double> Matrix::Inverse(
  const boost::numeric::ublas::matrix<double>& m)
{
  assert(m.size1() == m.size2() && "Can only calculate the inverse of square matrices");
  switch(m.size1())
  {
    case 1:
    {
      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;
    }
    case 2:
    {
      assert(m.size1() == 2 && m.size2() == 2 && "Only for 2x2 matrices");
      const double a = m(0,0);
      const double b = m(0,1);
      const double c = m(1,0);
      const double d = m(1,1);
      const double determinant = (a * d) - (b * c);
      boost::numeric::ublas::matrix<double> n(2,2);
      n(0,0) =  d / determinant;
      n(0,1) = -b / determinant;
      n(1,0) = -c / determinant;
      n(1,1) =  a / determinant;
      return n;
    }
    default:
      assert(!"Should not get here: unsupported matrix size");
      throw std::runtime_error("Unsupported matrix size");
  }
}

void Matrix::Test()
{
  {
    static bool is_tested = false;
    if (is_tested) return;
    is_tested = true;
  }
  using boost::numeric::ublas::matrix;
  using boost::numeric::ublas::vector;
  {
    ///[1,4]
    ///[2,5]
    ///[3,6]
    const matrix<int> m = CreateMatrix(3,2, {1,2,3,4,5,6} );
    assert(m(0,0) == 1);
    assert(m(1,0) == 2);
    assert(m(2,0) == 3);
    assert(m(0,1) == 4);
    assert(m(1,1) == 5);
    assert(m(2,1) == 6);
  }
  {
    /// [ 1.0 2.0 ] -1    [ -2.0   1.0 ]
    /// [ 3.0 4.0 ]     = [  1.5  -0.5 ]
    const matrix<double> m = CreateMatrix(2,2, {1.0,3.0,2.0,4.0} );
    assert(m(0,0) == 1.0);
    assert(m(1,0) == 3.0);
    assert(m(0,1) == 2.0);
    assert(m(1,1) == 4.0);
    const matrix<double> n = Inverse(m);
    const double epsilon = 0.0000001; //Rounding error
    assert(n(0,0) > -2.0 - epsilon && n(0,0) < -2.0 + epsilon);
    assert(n(1,0) >  1.5 - epsilon && n(1,0) <  1.5 + epsilon);
    assert(n(0,1) >  1.0 - epsilon && n(0,1) <  1.0 + epsilon);
    assert(n(1,1) > -0.5 - epsilon && n(1,1) < -0.5 + epsilon);
  }
}

 

 

 

 

 

qtmain.cpp

 

#ifdef _WIN32
#undef __STRICT_ANSI__
#endif

#include <QtGui/QApplication>
#include "qtmaindialog.h"

int main(int argc, char *argv[])
{
  QApplication a(argc, argv);
  QtMainDialog w;
  w.show();
  
  return a.exec();
}

 

 

 

 

 

qtmaindialog.h

 

#ifndef QTMAINDIALOG_H
#define QTMAINDIALOG_H

#ifdef _WIN32
#undef __STRICT_ANSI__
#endif

#include <QDialog>

namespace Ui {
  class QtMainDialog;
}

struct QwtPlotCurve;

class QtMainDialog : public QDialog
{
  Q_OBJECT
  
public:
  explicit QtMainDialog(QWidget *parent = 0);
  ~QtMainDialog();
  
private:
  Ui::QtMainDialog *ui;

  QwtPlotCurve * const m_curve_v_estimate;
  QwtPlotCurve * const m_curve_v_measured;
  QwtPlotCurve * const m_curve_v_real;
  QwtPlotCurve * const m_curve_x_estimate;
  QwtPlotCurve * const m_curve_x_measured;
  QwtPlotCurve * const m_curve_x_real;

};

#endif // QTMAINDIALOG_H

 

 

 

 

 

qtmaindialog.cpp

 

#ifdef _WIN32
#undef __STRICT_ANSI__
#endif

#include "qtmaindialog.h"

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

#include "qwt_plot_curve.h"
#include "qwt_plot_grid.h"
#include "qwt_plot_zoomer.h"

#include "maindialog.h"
#include "matrix.h"
#include "ui_qtmaindialog.h"

QtMainDialog::QtMainDialog(QWidget *parent) :
  QDialog(parent),
  ui(new Ui::QtMainDialog),
  m_curve_v_estimate(new QwtPlotCurve),
  m_curve_v_measured(new QwtPlotCurve),
  m_curve_v_real(new QwtPlotCurve),
  m_curve_x_estimate(new QwtPlotCurve),
  m_curve_x_measured(new QwtPlotCurve),
  m_curve_x_real(new QwtPlotCurve)

{
  ui->setupUi(this);

  ui->plot_x->setTitle("Position");
  ui->plot_x->setAxisTitle(QwtPlot::xBottom,"Time");
  ui->plot_x->setAxisTitle(QwtPlot::yLeft,"Position");
  m_curve_x_estimate->setTitle("Estimated");
  m_curve_x_estimate->attach(ui->plot_x);
  m_curve_x_estimate->setStyle(QwtPlotCurve::Lines);
  m_curve_x_estimate->setPen(QPen(QColor(255,0,0)));
  m_curve_x_measured->setTitle("Measured");
  m_curve_x_measured->attach(ui->plot_x);
  m_curve_x_measured->setStyle(QwtPlotCurve::Lines);
  m_curve_x_measured->setPen(QPen(QColor(0,255,0)));
  m_curve_x_real->setTitle("Real");
  m_curve_x_real->attach(ui->plot_x);
  m_curve_x_real->setStyle(QwtPlotCurve::Lines);
  m_curve_x_real->setPen(QPen(QColor(0,0,255)));

  ui->plot_v->setTitle("Velocity");
  ui->plot_v->setAxisTitle(QwtPlot::xBottom,"Time");
  ui->plot_v->setAxisTitle(QwtPlot::yLeft,"Velocity");
  m_curve_v_estimate->setTitle("Estimated");
  m_curve_v_estimate->attach(ui->plot_v);
  m_curve_v_estimate->setStyle(QwtPlotCurve::Lines);
  m_curve_v_estimate->setPen(QPen(QColor(255,0,0)));
  m_curve_v_measured->setTitle("Measured");
  m_curve_v_measured->attach(ui->plot_v);
  m_curve_v_measured->setStyle(QwtPlotCurve::Lines);
  m_curve_v_measured->setPen(QPen(QColor(0,255,0)));
  m_curve_v_real->setTitle("Real");
  m_curve_v_real->attach(ui->plot_v);
  m_curve_v_real->setStyle(QwtPlotCurve::Lines);
  m_curve_v_real->setPen(QPen(QColor(0,0,255)));

  //Add grids
  { QwtPlotGrid * const grid = new QwtPlotGrid; grid->setPen(QPen(QColor(196,196,196))); grid->attach(ui->plot_x); }
  { QwtPlotGrid * const grid = new QwtPlotGrid; grid->setPen(QPen(QColor(196,196,196))); grid->attach(ui->plot_v); }
  //Add zoomers
  {
    new QwtPlotZoomer(ui->plot_x->canvas());
    new QwtPlotZoomer(ui->plot_v->canvas());
  }

  //Do the sim
  const double t = 0.1;
  const double acceleration = 1.0;

  //The real state vector
  //[ position ]
  //[ velocity ]
  const boost::numeric::ublas::vector<double> init_x_real = Matrix::CreateVector( { 0.0, 0.0 } );

  //Real measurement noise
  //[ standard deviation of noise in position ]   [ standard deviation of noise in GPS                       ]
  //[ standard deviation of noise in velocity ] = [ standard deviation of noise in defect/unused speedometer ]
  const boost::numeric::ublas::vector<double> x_real_measurement_noise = Matrix::CreateVector( { 10.0, 10000000.0 } );

  //Guess of the state matrix
  //Position and velocity guess is way off on purpose
  //[ position ]
  //[ velocity ]
  const boost::numeric::ublas::vector<double> x_first_guess = Matrix::CreateVector( { 100.0, 10.0 } );

  //Guess of the covariances
  //[ 1.0   0.0 ]
  //[ 0.0   1.0 ]
  const boost::numeric::ublas::matrix<double> p_first_guess = Matrix::CreateMatrix(2,2, { 1.0, 0.0, 0.0, 1.0 } );

  //Effect of inputs on state
  //Input = gas pedal, which gives acceleration 'a'
  //[ 1.0   0.5 * t * t ]   [teleportation (not used)                 x = 0.5 * a * t * t ]
  //[ 0.0   t           ] = [no effect of teleportation on velocity   v = a * t           ]
  const boost::numeric::ublas::matrix<double> control = Matrix::CreateMatrix(2,2, { 1.0, 0.0, 0.5 * t * t, t } );

  //Estimated measurement noise
  //[ 10.0          0.0 ]   [ Estimated noise in GPS   ?                                                     ]
  //[  0.0   10000000.0 ] = [ ?                        Estimated noise in speedometer (absent in this setup) ]
  const boost::numeric::ublas::matrix<double> measurement_noise = Matrix::CreateMatrix(2,2, { 10.0, 0.0, 0.0, 10000000.0 } );

  //Observational matrix
  //[ 1.0   0.0 ]   [GPS measurement   ?                                         ]
  //[ 0.0   0.0 ] = [?                 Speedometer (absent/unused in this setup) ]
  const boost::numeric::ublas::matrix<double> observation = Matrix::CreateMatrix(2,2, { 1.0, 0.0, 0.0, 0.0 } );

  //Real process noise
  //[ 0.001 ]   [ noise in position ]
  //[ 0.001 ] = [ noise in velocity ]
  const boost::numeric::ublas::vector<double> real_process_noise = Matrix::CreateVector( {0.01,0.01} );

  //Estimated process noise covariance
  //[ 0.01   0.01 ]
  //[ 0.01   0.01 ]
  const boost::numeric::ublas::matrix<double> process_noise = Matrix::CreateMatrix(2,2,{0.01,0.01,0.01,0.01});

  //State transition matrix, the effect of the current state on the next
  //[ 1.0     t ]   [ position keeps its value             a velocity changes the position ]
  //[ 0.0   1.0 ] = [ position has no effect on velocity   a velocity keeps its value      ]
  const boost::numeric::ublas::matrix<double> state_transition = Matrix::CreateMatrix(2,2,{1.0,0.0,t,1.0});

  const int time = 250;
  const MainDialog d(
    time,
    acceleration,
    control,
    measurement_noise,
    observation,
    p_first_guess,
    process_noise,
    state_transition,
    init_x_real,
    real_process_noise,
    x_first_guess,
    x_real_measurement_noise);

  //Display data
  {
    const boost::numeric::ublas::matrix<double>& data = d.GetData();
    std::vector<double> time_series(time);
    std::vector<double> v_estimate(time);
    std::vector<double> v_measured(time);
    std::vector<double> v_real(time);
    std::vector<double> x_estimate(time);
    std::vector<double> x_measured(time);
    std::vector<double> x_real(time);

    for (int row=0; row!=time; ++row)
    {
      time_series[row] = static_cast<double>(row);
      x_real[     row] = data(row,0);
      x_measured[ row] = data(row,1);
      x_estimate[ row] = data(row,2);
      v_real[     row] = data(row,3);
      v_measured[ row] = data(row,4);
      v_estimate[ row] = data(row,5);
    }
    #ifdef _WIN32
    m_curve_v_estimate->setData(new QwtPointArrayData(&time_series[0],&v_estimate[0],time_series.size()));
    m_curve_v_measured->setData(new QwtPointArrayData(&time_series[0],&v_measured[0],time_series.size()));
    m_curve_v_real->setData(new QwtPointArrayData(&time_series[0],&v_real[0],time_series.size()));
    m_curve_x_estimate->setData(new QwtPointArrayData(&time_series[0],&x_estimate[0],time_series.size()));
    m_curve_x_measured->setData(new QwtPointArrayData(&time_series[0],&x_measured[0],time_series.size()));
    m_curve_x_real->setData(new QwtPointArrayData(&time_series[0],&x_real[0],time_series.size()));
    #else
    m_curve_v_estimate->setData(&time_series[0],&v_estimate[0],time_series.size());
    m_curve_v_measured->setData(&time_series[0],&v_measured[0],time_series.size());
    m_curve_v_real->setData(&time_series[0],&v_real[0],time_series.size());
    m_curve_x_estimate->setData(&time_series[0],&x_estimate[0],time_series.size());
    m_curve_x_measured->setData(&time_series[0],&x_measured[0],time_series.size());
    m_curve_x_real->setData(&time_series[0],&x_real[0],time_series.size());
    #endif
  }
  ui->plot_v->replot();
  ui->plot_x->replot();

}

QtMainDialog::~QtMainDialog()
{
  delete m_curve_v_estimate;
  delete m_curve_v_measured;
  delete m_curve_v_real;
  delete m_curve_x_estimate;
  delete m_curve_x_measured;
  delete m_curve_x_real;
  delete ui;
}

 

 

 

 

 

whitenoisesystem.h

 

#ifndef WHITENOISESYSTEM_H
#define WHITENOISESYSTEM_H

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

struct WhiteNoiseSystem
{
  WhiteNoiseSystem(
    const boost::numeric::ublas::matrix<double>& control,
    const boost::numeric::ublas::vector<double>& initial_state,
    const boost::numeric::ublas::vector<double>& real_measurement_noise,
    const boost::numeric::ublas::vector<double>& real_process_noise,
    const boost::numeric::ublas::matrix<double>& state_transition);

  ///Update reality, that is, let the real system (i.e. reality) go to its next state,
  ///without any input
  void GoToNextState();

  ///Update reality, that is, let the real system (i.e. reality) go to its next state
  void GoToNextState(const boost::numeric::ublas::vector<double>& input);

  ///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>& PeekAtRealState() const { return m_current_state; }

  private:
  ///The control matrix to determine the influence of the input (in GoToNextState)
  const boost::numeric::ublas::matrix<double> m_control;

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

  ///The standard deviation of the noise in the state transition (in GoToNextState)
  const boost::numeric::ublas::vector<double> m_process_noise;

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

  ///The state transitions in the system, used in GoToNextState
  const boost::numeric::ublas::matrix<double> m_state_transition;

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

 

#ifdef _WIN32
#undef __STRICT_ANSI__
#endif

#include "whitenoisesystem.h"

#include <iostream>
#include <boost/numeric/ublas/io.hpp>
#include <boost/random/lagged_fibonacci.hpp>
#include <boost/random/normal_distribution.hpp>

WhiteNoiseSystem::WhiteNoiseSystem(
  const boost::numeric::ublas::matrix<double>& control,
  const boost::numeric::ublas::vector<double>& initial_state,
  const boost::numeric::ublas::vector<double>& real_measurement_noise,
  const boost::numeric::ublas::vector<double>& real_process_noise,
  const boost::numeric::ublas::matrix<double>& state_transition)
  : m_control(control),
    m_current_state(initial_state),
    m_process_noise(real_process_noise),
    m_real_measurement_noise(real_measurement_noise),
    m_state_transition(state_transition)
{
  #ifndef NDEBUG
  //Check for correct dimensionality
  const auto sz = initial_state.size();
  assert(m_control.size1() == sz);
  assert(m_control.size2() == sz);
  assert(m_current_state.size() == sz);
  assert(m_process_noise.size() == sz);
  assert(m_real_measurement_noise.size() == sz);
  assert(m_state_transition.size1() == sz);
  assert(m_state_transition.size2() == sz);
  #endif
}

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

void WhiteNoiseSystem::GoToNextState()
{
  //Create a no-input vector
  const auto sz = m_current_state.size();
  boost::numeric::ublas::vector<double> input(sz,0.0);
  return GoToNextState(input);
}

void WhiteNoiseSystem::GoToNextState(const boost::numeric::ublas::vector<double>& input)
{
  //First do a perfect transition
  m_current_state
    = boost::numeric::ublas::prod(m_state_transition,m_current_state)
    + boost::numeric::ublas::prod(m_control,input);
  //Add process noise
  const auto sz = m_current_state.size();
  assert(m_current_state.size() == m_process_noise.size());
  for (std::size_t i = 0; i!=sz; ++i)
  {
    m_current_state(i) = GetRandomNormal(m_current_state(i),m_process_noise(i));
  }
}

const boost::numeric::ublas::vector<double> WhiteNoiseSystem::Measure() const
{
  const auto sz = m_current_state.size();
  assert(m_current_state.size() == m_real_measurement_noise.size());
  boost::numeric::ublas::vector<double> measured(sz);
  for (std::size_t i = 0; i!=sz; ++i)
  {
    measured(i) = GetRandomNormal(m_current_state(i),m_real_measurement_noise(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