Go back to Richel Bilderbeek's homepage.

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

 

 

 

 

 

(C++) KalmanFilterExample8

 

Kalman filter example 8 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: CppKalmanFilterExample8Console.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-mgw44-mt-1_50 \
    -lboost_filesystem-mgw44-mt-1_50 \
    -lboost_regex-mgw44-mt-1_50
}


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

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

 

 

 

 

 

Qt project file: CppKalmanFilterExample8Desktop.pro

 

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

#Prevents error:
#/my_boost_folder/boost/type_traits/detail/has_binary_operator.hp:50: Parse error at "BOOST_JOIN"
DEFINES += BOOST_TT_HAS_OPERATOR_HPP_INCLUDED

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
}

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


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)
  if (Matrix::CalcDeterminant(innovation_covariance) == 0.0)
  {
    throw std::runtime_error("Innovation covariance became degenerate");
  }
  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;

  //The name of the states:
  //x: position
  //v: velocity
  const std::vector<std::string> state_names = { "x", "v" };

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

  //There is a constant push on the gas pedal. This has no direct effect on the position,
  //but it does increase velocity with accelation every state transition
  const double acceleration = 1.0;
  const boost::numeric::ublas::vector<double> input = Matrix::CreateVector( {0.0,acceleration} );

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

  //Display header
  {
    const boost::numeric::ublas::vector<std::string> header = d.GetHeader(state_names);
    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(state_names).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 boost::numeric::ublas::matrix<double>& control,
    const boost::numeric::ublas::vector<double>& input,
    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 std::vector<std::string>& state_names,
    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 from the states
  static const boost::numeric::ublas::vector<std::string> GetHeader(const std::vector<std::string>& state_names);

  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 boost::numeric::ublas::matrix<double>& control,
    const boost::numeric::ublas::vector<double>& input,
    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 std::vector<std::string>& state_names,
    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 <boost/numeric/ublas/functional.hpp>
#include "kalmanfilter.h"
#include "matrix.h"
#include "whitenoisesystem.h"

MainDialog::MainDialog(
  const int time,
  const boost::numeric::ublas::matrix<double>& control,
  const boost::numeric::ublas::vector<double>& input,
  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 std::vector<std::string>& state_names,
  const boost::numeric::ublas::vector<double>& x_first_guess,
  const boost::numeric::ublas::vector<double>& x_real_measurement_noise)
  : m_data(
    CreateData(
      time,
      control,
      input,
      measurement_noise,
      observation,
      p_first_guess,
      process_noise,
      state_transition,
      init_x_real,
      real_process_noise,
      state_names,
      x_first_guess,
      x_real_measurement_noise
      )
    )
{



}

const boost::numeric::ublas::matrix<double> MainDialog::CreateData(
  const int time,
  const boost::numeric::ublas::matrix<double>& control,
  const boost::numeric::ublas::vector<double>& input,
  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 std::vector<std::string>& state_names,
  const boost::numeric::ublas::vector<double>& x_first_guess,
  const boost::numeric::ublas::vector<double>& x_real_measurement_noise)

{
  Matrix::Test();
  assert(state_names.size() == init_x_real.size());

  using boost::numeric::ublas::matrix;
  using boost::numeric::ublas::vector;
  const int n_states = init_x_real.size();

  //The resulting matrix, has time rows and states * three (real,measured,Kalman) columns
  matrix<double> data(time,n_states * 3);
  assert(time == static_cast<int>(data.size1()));
  assert(n_states * 3 == static_cast<int>(data.size2()));
  assert(GetHeader(state_names).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
    try
    {
      k.SupplyMeasurementAndInput(z_measured,input);
    }
    catch (std::runtime_error& e)
    {
      //Happens when innovation covariance becomes degenerate
      //(that is, its determinant is zero)
      return data;
    }
    //Display what the filter predicts
    const vector<double> x_est_last = k.Predict();
    for (int j=0; j!=n_states; ++j)
    {
      assert(i < static_cast<int>(data.size1()));
      assert((j*3)+2 < static_cast<int>(data.size2()));
      assert(j < static_cast<int>(s.PeekAtRealState().size()));
      assert(j < static_cast<int>(z_measured.size()));
      assert(j < static_cast<int>(x_est_last.size()));
      data(i,(j*3)+0) = s.PeekAtRealState()(j);
      data(i,(j*3)+1) = z_measured(j);
      data(i,(j*3)+2) = x_est_last(j);
    }
  }
  return data;
}

const boost::numeric::ublas::vector<std::string> MainDialog::GetHeader(
  const std::vector<std::string>& state_names)
{
  const int n_states = static_cast<int>(state_names.size());
  boost::numeric::ublas::vector<std::string> v(n_states * 3);
  for (int i=0; i!=n_states; ++i)
  {
    assert((i*3)+2 < static_cast<int>(v.size()));
    v((i*3)+0) = state_names[i] + "_real";
    v((i*3)+1) = state_names[i] + "_measured";
    v((i*3)+2) = state_names[i] + "_Kalman";
  }
  assert(static_cast<int>(state_names.size()) * 3 == static_cast<int>(v.size()));
  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
{
  ///Calculate the determinant
  //Adapted from the code Maik Beckmann posted at
  //  http://boost.2283326.n4.nabble.com/How-to-compute-determinant-td2710896.html
  static double CalcDeterminant(boost::numeric::ublas::matrix<double> m);

  ///Chop returns a std::vector of sub-matrices
  ///[ A at [0]   B at [1] ]
  ///[ C at [2]   D at [4] ]
  static const std::vector<boost::numeric::ublas::matrix<double> > Chop(
    const boost::numeric::ublas::matrix<double>& m);

  ///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 random-filled matrix
  static const boost::numeric::ublas::matrix<double> CreateRandomMatrix(
    const std::size_t n_rows, const std::size_t n_cols);

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

  ///Check if two doubles are about equal
  static bool IsAboutEqual(const double a, const double b);

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

  ///Unchop merges the 4 std::vector of sub-matrices produced by Chop
  static const boost::numeric::ublas::matrix<double> Unchop(
    const std::vector<boost::numeric::ublas::matrix<double> >& v);

};

#endif // MATRIX_H

 

 

 

 

 

matrix.cpp

 

#ifdef _WIN32
#undef __STRICT_ANSI__
#endif

#include "matrix.h"

#include <iostream>

#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>

double Matrix::CalcDeterminant(boost::numeric::ublas::matrix<double> m)
{
  assert(m.size1() == m.size2() && "Can only calculate the determinant of square matrices");
  boost::numeric::ublas::permutation_matrix<std::size_t> pivots(m.size1() );

  const int is_singular = boost::numeric::ublas::lu_factorize(m, pivots);

  if (is_singular) return 0.0;

  double d = 1.0;
  const std::size_t sz = pivots.size();
  for (std::size_t i=0; i != sz; ++i)
  {
    if (pivots(i) != i)
    {
      d *= -1.0;
    }
    d *= m(i,i);
  }
  return d;
}

const std::vector<boost::numeric::ublas::matrix<double> > Matrix::Chop(
  const boost::numeric::ublas::matrix<double>& m)
{
  using boost::numeric::ublas::range;
  using boost::numeric::ublas::matrix;
  using boost::numeric::ublas::matrix_range;
  std::vector<matrix<double> > v;
  v.reserve(4);
  const int midy = m.size1() / 2;
  const int midx = m.size2() / 2;
  const matrix_range<const matrix<double> > top_left(    m,range(0   ,midy     ),range(0   ,midx     ));
  const matrix_range<const matrix<double> > bottom_left( m,range(midy,m.size1()),range(0   ,midx     ));
  const matrix_range<const matrix<double> > top_right(   m,range(0   ,midy     ),range(midx,m.size2()));
  const matrix_range<const matrix<double> > bottom_right(m,range(midy,m.size1()),range(midx,m.size2()));
  v.push_back(matrix<double>(top_left));
  v.push_back(matrix<double>(top_right));
  v.push_back(matrix<double>(bottom_left));
  v.push_back(matrix<double>(bottom_right));
  return v;
}

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::matrix<double> Matrix::CreateRandomMatrix(const std::size_t n_rows, const std::size_t n_cols)
{
  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) = static_cast<double>(std::rand()) / static_cast<double>(RAND_MAX);
    }
  }
  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");
      const double determinant = CalcDeterminant(m);
      assert(determinant != 0.0);
      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 / determinant;
      return n;
    }
    case 2:
    {
      assert(m.size1() == 2 && m.size2() == 2 && "Only for 2x2 matrices");
      const double determinant = CalcDeterminant(m);
      assert(determinant != 0.0);
      const double a = m(0,0);
      const double b = m(0,1);
      const double c = m(1,0);
      const double d = m(1,1);
      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;
    }
    case 3:
    {
      assert(m.size1() == 3 && m.size2() == 3 && "Only for 3x3 matrices");
      const double determinant = CalcDeterminant(m);
      assert(determinant != 0.0);
      const double a = m(0,0);
      const double b = m(0,1);
      const double c = m(0,2);
      const double d = m(1,0);
      const double e = m(1,1);
      const double f = m(1,2);
      const double g = m(2,0);
      const double h = m(2,1);
      const double k = m(2,2);
      boost::numeric::ublas::matrix<double> n(3,3);
      const double new_a =  ((e*k)-(f*h)) / determinant;
      const double new_b = -((d*k)-(f*g)) / determinant;
      const double new_c =  ((d*h)-(e*g)) / determinant;
      const double new_d = -((b*k)-(c*h)) / determinant;
      const double new_e =  ((a*k)-(c*g)) / determinant;
      const double new_f = -((a*h)-(b*g)) / determinant;
      const double new_g =  ((b*f)-(c*e)) / determinant;
      const double new_h = -((a*f)-(c*d)) / determinant;
      const double new_k =  ((a*e)-(b*d)) / determinant;
      n(0,0) = new_a;
      n(1,0) = new_b;
      n(2,0) = new_c;
      n(0,1) = new_d;
      n(1,1) = new_e;
      n(2,1) = new_f;
      n(0,2) = new_g;
      n(1,2) = new_h;
      n(2,2) = new_k;
      return n;
    }
    default:
    {
      //Use blockwise inversion
      //Matrix::Chop returns a std::vector
      //[ A at [0]   B at [1] ]
      //[ C at [2]   D at [4] ]
      const std::vector<boost::numeric::ublas::matrix<double> > v = Chop(m);
      const boost::numeric::ublas::matrix<double>& a = v[0];
      assert(a.size1() == a.size2());
      const boost::numeric::ublas::matrix<double>  a_inv = Inverse(a);
      const boost::numeric::ublas::matrix<double>& b = v[1];
      const boost::numeric::ublas::matrix<double>& c = v[2];
      const boost::numeric::ublas::matrix<double>& d = v[3];
      const boost::numeric::ublas::matrix<double> term
        = d
        - prod(
            boost::numeric::ublas::matrix<double>(prod(c,a_inv)),
            b
          );
      assert(term.size1() == term.size2());
      const boost::numeric::ublas::matrix<double> term_inv = Inverse(term);
      const boost::numeric::ublas::matrix<double> new_a
        = a_inv
        + boost::numeric::ublas::matrix<double>(prod(
            boost::numeric::ublas::matrix<double>(prod(
              boost::numeric::ublas::matrix<double>(prod(
                boost::numeric::ublas::matrix<double>(prod(
                  a_inv,
                  b)),
                term_inv)),
             c)),
            a_inv));

      const boost::numeric::ublas::matrix<double> new_b
        =
        - boost::numeric::ublas::matrix<double>(prod(
            boost::numeric::ublas::matrix<double>(prod(
              a_inv,
              b)),
            term_inv));

      const boost::numeric::ublas::matrix<double> new_c
        =
        - boost::numeric::ublas::matrix<double>(prod(
            boost::numeric::ublas::matrix<double>(prod(
              term_inv,
              c)),
            a_inv));

      const boost::numeric::ublas::matrix<double> new_d = term_inv;
      const std::vector<boost::numeric::ublas::matrix<double> > w = { new_a, new_b, new_c, new_d };
      const boost::numeric::ublas::matrix<double> result = Unchop(w);
      return result;
    }
  }
}


bool Matrix::IsAboutEqual(const double a, const double b)
{
  const double epsilon = 0.000001; //Rounding error
  return a - epsilon < b && a + epsilon > b;
}

void Matrix::Test()
{
  {
    static bool is_tested = false;
    if (is_tested) return;
    is_tested = true;
  }
  using boost::numeric::ublas::detail::equals;
  using boost::numeric::ublas::matrix;
  using boost::numeric::ublas::prod;
  using boost::numeric::ublas::vector;
  //Test CreateMatrix
  {
    // [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);
  }
  //Test Chop on 3x3
  {
    //                     [ 1.0 ] | [ 2.0   3.0 ]
    // [ 1.0 2.0 3.0 ]     --------+--------------
    // [ 4.0 5.0 6.0 ]     [ 4.0 ] | [ 5.0   6.0 ]
    // [ 7.0 8.0 9.0 ] ->  [ 7.0 ] | [ 8.0   9.0 ]
    const matrix<double> m = CreateMatrix(3,3, {1.0,4.0,7.0,2.0,5.0,8.0,3.0,6.0,9.0} );
    assert(m(0,0) == 1.0); assert(m(0,1) == 2.0); assert(m(0,2) == 3.0);
    assert(m(1,0) == 4.0); assert(m(1,1) == 5.0); assert(m(1,2) == 6.0);
    assert(m(2,0) == 7.0); assert(m(2,1) == 8.0); assert(m(2,2) == 9.0);
    const std::vector<matrix<double> > n = Chop(m);
    assert(n.size() == 4);
    std::clog
      << "m   : " << m    << '\n'
      << "n[0]: " << n[0] << '\n'
      << "n[1]: " << n[1] << '\n'
      << "n[2]: " << n[2] << '\n'
      << "n[3]: " << n[3] << '\n';
    assert(n[0].size1() == 1);
    assert(n[0].size2() == 1);
    assert(n[1].size1() == 1);
    assert(n[1].size2() == 2);
    assert(n[2].size1() == 2);
    assert(n[2].size2() == 1);
    assert(n[3].size1() == 2);
    assert(n[3].size2() == 2);
    assert(n[0].size1() + n[2].size1() == m.size1());
    assert(n[1].size1() + n[3].size1() == m.size1());
    assert(n[0].size2() + n[1].size2() == m.size2());
    assert(n[2].size2() + n[3].size2() == m.size2());
  }
  //Test Chop on 5x5
  {
    const matrix<double> m = CreateMatrix(5,5,
      {
        1.0, 6.0,11.0,16.0,21.0,
        2.0, 7.0,12.0,17.0,22.0,
        3.0, 8.0,13.0,18.0,23.0,
        4.0, 9.0,14.0,19.0,24.0,
        5.0,10.0,15.0,20.0,25.0
      }
    );
    assert(m(0,0) ==  1.0); assert(m(0,1) ==  2.0); assert(m(0,2) ==  3.0); assert(m(0,3) ==  4.0); assert(m(0,4) ==  5.0);
    assert(m(1,0) ==  6.0); assert(m(1,1) ==  7.0); assert(m(1,2) ==  8.0); assert(m(1,3) ==  9.0); assert(m(1,4) == 10.0);
    assert(m(2,0) == 11.0); assert(m(2,1) == 12.0); assert(m(2,2) == 13.0); assert(m(2,3) == 14.0); assert(m(2,4) == 15.0);
    assert(m(3,0) == 16.0); assert(m(3,1) == 17.0); assert(m(3,2) == 18.0); assert(m(3,3) == 19.0); assert(m(3,4) == 20.0);
    assert(m(4,0) == 21.0); assert(m(4,1) == 22.0); assert(m(4,2) == 23.0); assert(m(4,3) == 24.0); assert(m(4,4) == 25.0);
    const std::vector<matrix<double> > n = Chop(m);
    assert(n.size() == 4);
    std::clog
      << "m   : " << m    << '\n'
      << "n[0]: " << n[0] << '\n'
      << "n[1]: " << n[1] << '\n'
      << "n[2]: " << n[2] << '\n'
      << "n[3]: " << n[3] << '\n';
    assert(n[0].size1() == 2);
    assert(n[0].size2() == 2);
    assert(n[1].size1() == 2);
    assert(n[1].size2() == 3);
    assert(n[2].size1() == 3);
    assert(n[2].size2() == 2);
    assert(n[3].size1() == 3);
    assert(n[3].size2() == 3);
    assert(n[0].size1() + n[2].size1() == m.size1());
    assert(n[1].size1() + n[3].size1() == m.size1());
    assert(n[0].size2() + n[1].size2() == m.size2());
    assert(n[2].size2() + n[3].size2() == m.size2());
  }
  //Test Unchop
  {
    //Check 0x0 to and including 9x9 matrices
    for (std::size_t n_rows = 0; n_rows!=10; ++n_rows)
    {
      for (std::size_t n_cols = 0; n_cols!=10; ++n_cols)
      {
        //Epsilon is more or less the smallest round-off error
        const double epsilon = std::numeric_limits<double>::epsilon();

        //Create a random matrix
        const matrix<double> m = Matrix::CreateRandomMatrix(n_rows,n_cols);

        //Assume it is found identical to itself
        assert(equals(m,m,epsilon,epsilon));

        //Chop and unchop the input matrix
        const matrix<double> n = Matrix::Unchop(Chop(m));

        //Assume input matrix and result are identical
        assert(equals(m,n,epsilon,epsilon));
      }
    }
  }
  //Test Inverse on 2x2 matrix
  {
    // [ 1.0 2.0 ] -1    [ -2.0   1.0 ]
    // [ 3.0 4.0 ]     = [  1.5  -0.5 ]
    const matrix<double> m = Matrix::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 = Matrix::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);
    assert(prod(m,n)(0,0) > 1.0 - epsilon && prod(m,n)(0,0) < 1.0 + epsilon);
    assert(prod(m,n)(1,0) > 0.0 - epsilon && prod(m,n)(1,0) < 0.0 + epsilon);
    assert(prod(m,n)(0,1) > 0.0 - epsilon && prod(m,n)(0,1) < 0.0 + epsilon);
    assert(prod(m,n)(1,1) > 1.0 - epsilon && prod(m,n)(1,1) < 1.0 + epsilon);
  }


  {
    // [ 1.0 2.0 3.0] -1    [ -24.0   18.0   5.0]
    // [ 0.0 1.0 4.0]       [  20.0  -15.0  -4.0]
    // [ 5.0 6.0 0.0]     = [ - 5.0    4.0   1.0]
    const matrix<double> m = Matrix::CreateMatrix(3,3, {1.0,0.0,5.0,2.0,1.0,6.0,3.0,4.0,0.0} );
    assert(m(0,0) == 1.0); assert(m(0,1) == 2.0); assert(m(0,2) == 3.0);
    assert(m(1,0) == 0.0); assert(m(1,1) == 1.0); assert(m(1,2) == 4.0);
    assert(m(2,0) == 5.0); assert(m(2,1) == 6.0); assert(m(2,2) == 0.0);
    const matrix<double> n = Matrix::Inverse(m);
    const double epsilon = 0.0001; //Rounding error
    assert(n(0,0) > -24.0 - epsilon && n(0,0) < -24.0 + epsilon);
    assert(n(1,0) >  20.0 - epsilon && n(1,0) <  20.0 + epsilon);
    assert(n(2,0) > - 5.0 - epsilon && n(2,0) < - 5.0 + epsilon);
    assert(n(0,1) >  18.0 - epsilon && n(0,1) <  18.0 + epsilon);
    assert(n(1,1) > -15.0 - epsilon && n(1,1) < -15.0 + epsilon);
    assert(n(2,1) >   4.0 - epsilon && n(2,1) <   4.0 + epsilon);
    assert(n(0,2) >   5.0 - epsilon && n(0,2) <   5.0 + epsilon);
    assert(n(1,2) >  -4.0 - epsilon && n(1,2) < - 4.0 + epsilon);
    assert(n(2,2) >   1.0 - epsilon && n(2,2) <   1.0 + epsilon);
    const matrix<double> i = prod(m,n);
    assert(i(0,0) > 1.0 - epsilon && i(0,0) < 1.0 + epsilon);
    assert(i(1,0) > 0.0 - epsilon && i(1,0) < 0.0 + epsilon);
    assert(i(2,0) > 0.0 - epsilon && i(2,0) < 0.0 + epsilon);
    assert(i(0,1) > 0.0 - epsilon && i(0,1) < 0.0 + epsilon);
    assert(i(1,1) > 1.0 - epsilon && i(1,1) < 1.0 + epsilon);
    assert(i(2,1) > 0.0 - epsilon && i(2,1) < 0.0 + epsilon);
    assert(i(0,2) > 0.0 - epsilon && i(0,2) < 0.0 + epsilon);
    assert(i(1,2) > 0.0 - epsilon && i(1,2) < 0.0 + epsilon);
    assert(i(2,2) > 1.0 - epsilon && i(2,2) < 1.0 + epsilon);
  }
  //Test Inverse on 3x3 matrix
  {
    // [ 1.0 2.0 3.0] -1
    // [ 4.0 4.0 6.0]
    // [ 7.0 8.0 9.0]
    // Note: cannot make the center value equal to 5.0, as this makes
    // the matrix un-invertible (the determinant becomes equal to zero)
    const matrix<double> m = Matrix::CreateMatrix(3,3, {1.0,4.0,7.0,2.0,4.0,8.0,3.0,6.0,9.0} );
    assert(m(0,0) == 1.0); assert(m(0,1) == 2.0); assert(m(0,2) == 3.0);
    assert(m(1,0) == 4.0); assert(m(1,1) == 4.0); assert(m(1,2) == 6.0);
    assert(m(2,0) == 7.0); assert(m(2,1) == 8.0); assert(m(2,2) == 9.0);
    const matrix<double> n = Matrix::Inverse(m);
    const double epsilon = 0.00001; //Rounding error
    const matrix<double> i = prod(m,n);
    assert(i(0,0) > 1.0 - epsilon && i(0,0) < 1.0 + epsilon);
    assert(i(1,0) > 0.0 - epsilon && i(1,0) < 0.0 + epsilon);
    assert(i(2,0) > 0.0 - epsilon && i(2,0) < 0.0 + epsilon);
    assert(i(0,1) > 0.0 - epsilon && i(0,1) < 0.0 + epsilon);
    assert(i(1,1) > 1.0 - epsilon && i(1,1) < 1.0 + epsilon);
    assert(i(2,1) > 0.0 - epsilon && i(2,1) < 0.0 + epsilon);
    assert(i(0,2) > 0.0 - epsilon && i(0,2) < 0.0 + epsilon);
    assert(i(1,2) > 0.0 - epsilon && i(1,2) < 0.0 + epsilon);
    assert(i(2,2) > 1.0 - epsilon && i(2,2) < 1.0 + epsilon);
  }
  //Test Inverse on 4x4 matrix
  {
    const matrix<double> m = Matrix::CreateRandomMatrix(4,4);
    const matrix<double> n = Matrix::Inverse(m);
    const double epsilon = 0.00001; //Rounding error
    const matrix<double> i = prod(m,n);
    //Test if i is identity matrix
    assert(i(0,0) > 1.0 - epsilon && i(0,0) < 1.0 + epsilon);
    assert(i(1,0) > 0.0 - epsilon && i(1,0) < 0.0 + epsilon);
    assert(i(2,0) > 0.0 - epsilon && i(2,0) < 0.0 + epsilon);
    assert(i(3,0) > 0.0 - epsilon && i(3,0) < 0.0 + epsilon);
    assert(i(0,1) > 0.0 - epsilon && i(0,1) < 0.0 + epsilon);
    assert(i(1,1) > 1.0 - epsilon && i(1,1) < 1.0 + epsilon);
    assert(i(2,1) > 0.0 - epsilon && i(2,1) < 0.0 + epsilon);
    assert(i(3,1) > 0.0 - epsilon && i(3,1) < 0.0 + epsilon);
    assert(i(0,2) > 0.0 - epsilon && i(0,2) < 0.0 + epsilon);
    assert(i(1,2) > 0.0 - epsilon && i(1,2) < 0.0 + epsilon);
    assert(i(2,2) > 1.0 - epsilon && i(2,2) < 1.0 + epsilon);
    assert(i(3,2) > 0.0 - epsilon && i(3,2) < 0.0 + epsilon);
    assert(i(0,3) > 0.0 - epsilon && i(0,3) < 0.0 + epsilon);
    assert(i(1,3) > 0.0 - epsilon && i(1,3) < 0.0 + epsilon);
    assert(i(2,3) > 0.0 - epsilon && i(2,3) < 0.0 + epsilon);
    assert(i(3,3) > 1.0 - epsilon && i(3,3) < 1.0 + epsilon);
  }
  //Test Inverse on bigger matrices
  for (std::size_t sz = 5; sz!=20; ++sz)
  {
    const matrix<double> m = Matrix::CreateRandomMatrix(sz,sz);
    const matrix<double> n = Matrix::Inverse(m);
    const double epsilon = 0.00001; //Rounding error
    const matrix<double> i = prod(m,n);
    //Test if i is identity matrix
    for (std::size_t y = 0; y!=sz; ++y)
    {
      for (std::size_t x = 0; x!=sz; ++x)
      {
        assert(
             (x == y && i(y,x) > 1.0 - epsilon && i(y,x) < 1.0 + epsilon)
          || (x != y && i(y,x) > 0.0 - epsilon && i(y,x) < 0.0 + epsilon)
        );
      }
    }
  }
}

const boost::numeric::ublas::matrix<double> Matrix::Unchop(
  const std::vector<boost::numeric::ublas::matrix<double> >& v)
{
  //Chop returns a std::vector of sub-matrices
  //[ A at [0]   B at [1] ]
  //[ C at [2]   D at [4] ]
  using boost::numeric::ublas::range;
  using boost::numeric::ublas::matrix;
  using boost::numeric::ublas::matrix_range;
  assert(v.size() == 4);
  assert(v[0].size1() == v[1].size1());
  assert(v[2].size1() == v[3].size1());
  assert(v[0].size2() == v[2].size2());
  assert(v[1].size2() == v[3].size2());
  boost::numeric::ublas::matrix<double> m(v[0].size1() + v[2].size1(),v[0].size2() + v[1].size2());
  for (int quadrant=0; quadrant!=4; ++quadrant)
  {
    const boost::numeric::ublas::matrix<double>& w = v[quadrant];
    const std::size_t n_rows = v[quadrant].size1();
    const std::size_t n_cols = v[quadrant].size2();
    const int offset_x = quadrant % 2 ? v[0].size2() : 0;
    const int offset_y = quadrant / 2 ? v[0].size1() : 0;
    for (std::size_t row=0; row!=n_rows; ++row)
    {
      for (std::size_t col=0; col!=n_cols; ++col)
      {
        m(offset_y + row, offset_x + col) = w(row,col);
      }
    }
  }

  assert(v[0].size1() + v[2].size1() == m.size1());
  assert(v[1].size1() + v[3].size1() == m.size1());
  assert(v[0].size2() + v[1].size2() == m.size2());
  assert(v[2].size2() + v[3].size2() == m.size2());

  return m;
}

 

 

 

 

 

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

#include <QDialog>

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

namespace Ui {
  class QtMainDialog;
}

struct QwtPlot;
struct QwtPlotCurve;
struct QTableWidget;

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

  bool m_can_do_sim;
  std::vector<QwtPlotCurve *> m_curves;
  std::vector<QwtPlot *> m_plots;

  static const boost::numeric::ublas::matrix<double> ToMatrix(const QTableWidget * const table);
  static const boost::numeric::ublas::vector<double> ToVector(const QTableWidget * const table);
  void UpdateLegends();

private slots:
  void OnAnyChange();
  void on_box_n_states_valueChanged(int arg1);
  const std::vector<std::string> GetLegend() const;
  const std::vector<QTableWidget *> CollectMatrices() const;
  const std::vector<QTableWidget *> CollectVectors() const;
};

#endif // QTMAINDIALOG_H

 

 

 

 

 

qtmaindialog.cpp

 

#ifdef _WIN32
#undef __STRICT_ANSI__
#endif

#include "qtmaindialog.h"

#include <cassert>
#include <cstdlib>

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

#include "qwt_plot.h"
#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"

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


QtMainDialog::QtMainDialog(QWidget *parent) :
  QDialog(parent),
  ui(new Ui::QtMainDialog),
  m_can_do_sim(false)
{
  ui->setupUi(this);

  //Connect all tables to OnAnyChange
  {
    std::vector<QTableWidget *> v = CollectMatrices();
    {
      const std::vector<QTableWidget *> w = CollectVectors();
      std::copy(w.begin(),w.end(),std::back_inserter(v));
    }
    const std::size_t sz = v.size();
    for (std::size_t i=0; i!=sz; ++i)
    {
      QObject::connect(v[i],SIGNAL(cellChanged(int,int)),this,SLOT(OnAnyChange()));
    }

  }
  QObject::connect(ui->box_n_timesteps,SIGNAL(valueChanged(int)),this,SLOT(OnAnyChange()));

  on_box_n_states_valueChanged(2); //Will change table_states[1][0] to '?'
  //Put back the legend in table_states[1][0]
  {
    QTableWidgetItem * const item = new QTableWidgetItem;
    item->setText("v");
    ui->table_states->setItem(1,0,item);
  }
}

QtMainDialog::~QtMainDialog()
{
  {
    const std::size_t sz = m_curves.size();
    for (std::size_t i=0; i!=sz; ++i) delete m_curves[i];
  }
  {
    const std::size_t sz = m_plots.size();
    for (std::size_t i=0; i!=sz; ++i) delete m_plots[i];
  }
  delete ui;
}

const std::vector<QTableWidget *> QtMainDialog::CollectMatrices() const
{
  std::vector<QTableWidget *> v;
  v.push_back(ui->table_control);
  v.push_back(ui->table_init_covariance_estimate);
  v.push_back(ui->table_measurement_noise_estimate);
  v.push_back(ui->table_observation);
  v.push_back(ui->table_process_noise_covariance_estimate);
  v.push_back(ui->table_state_transition);
  return v;
}

const std::vector<QTableWidget *> QtMainDialog::CollectVectors() const
{
  std::vector<QTableWidget *> v;
  v.push_back(ui->table_init_state_estimate);
  v.push_back(ui->table_init_state_real);
  v.push_back(ui->table_input);
  v.push_back(ui->table_real_measurement_noise);
  v.push_back(ui->table_real_process_noise);
  v.push_back(ui->table_states);
  return v;
}

const std::vector<std::string> QtMainDialog::GetLegend() const
{
  std::vector<std::string> v;
  const int sz = ui->table_states->rowCount();
  for (int i=0; i!=sz; ++i)
  {
    const QTableWidgetItem * const item = ui->table_states->item(i,0);
    assert(item);
    const std::string s = item->text().toStdString();
    v.push_back(s);
  }
  return v;
}


void QtMainDialog::OnAnyChange()
{
  if (!m_can_do_sim)
  {
    //Sizes may be out of synch, as OnAnyChange is called
    //every resize in on_box_n_states_valueChanged.
    //on_box_n_states_valueChanged sets m_can_do_sim to false when working,
    //and sets it to true when done
    return;
  }
  m_can_do_sim = false;
  UpdateLegends();
  m_can_do_sim = true;
  try
  {
    //Do the sim
    const boost::numeric::ublas::vector<double> init_x_real = ToVector(ui->table_init_state_real);
    const boost::numeric::ublas::vector<double> input = ToVector(ui->table_input);
    const boost::numeric::ublas::vector<double> x_real_measurement_noise = ToVector(ui->table_real_measurement_noise);
    const boost::numeric::ublas::vector<double> x_first_guess = ToVector(ui->table_init_state_estimate);
    const boost::numeric::ublas::matrix<double> p_first_guess = ToMatrix(ui->table_init_covariance_estimate);
    const boost::numeric::ublas::matrix<double> control = ToMatrix(ui->table_control);
    const boost::numeric::ublas::matrix<double> measurement_noise_estimate = ToMatrix(ui->table_measurement_noise_estimate);
    const boost::numeric::ublas::matrix<double> observation = ToMatrix(ui->table_observation);
    const boost::numeric::ublas::vector<double> real_process_noise = ToVector(ui->table_real_process_noise);
    const boost::numeric::ublas::matrix<double> process_noise_estimate = ToMatrix(ui->table_process_noise_covariance_estimate);
    const boost::numeric::ublas::matrix<double> state_transition = ToMatrix(ui->table_state_transition);
    const std::vector<std::string> state_names = GetLegend();
    const int n_timesteps_desired = ui->box_n_timesteps->value();
    assert(state_names.size() == init_x_real.size());
    const MainDialog d(
      n_timesteps_desired,
      control,
      input,
      measurement_noise_estimate,
      observation,
      p_first_guess,
      process_noise_estimate,
      state_transition,
      init_x_real,
      real_process_noise,
      state_names,
      x_first_guess,
      x_real_measurement_noise);

    //Display data
    {
      //Convert data to a collection of std::vector, for QwtPlot to read
      const boost::numeric::ublas::matrix<double>& data = d.GetData();
      //n_timesteps may differ from n_timesteps_desired, because in the actual simulation
      //the innovation variance may become degenerate
      const int n_timesteps = data.size1(); //Number of rows
      const std::vector<std::vector<double> > vs = To2dVector(data);

      //Create time series
      std::vector<double> time_series(n_timesteps,0.0);
      for (int t=0; t!=n_timesteps; ++t) time_series[t] = static_cast<double>(t);

      //Put data on curves
      assert(vs.size() == m_curves.size());
      assert(n_timesteps_desired == static_cast<int>(vs[0].size()));
      assert(n_timesteps_desired == static_cast<int>(time_series.size()));
      const std::size_t n_curves = m_curves.size();
      for (std::size_t i=0; i!=n_curves; ++i)
      {
        const std::vector<double>& v = vs[i];
        assert(n_timesteps_desired == static_cast<int>(v.size()));
        #ifdef _WIN32
        m_curves[i]->setData(new QwtPointArrayData(&time_series[0],&v[0],time_series.size()));
        #else
        m_curves[i]->setData(&time_series[0],&v[0],time_series.size());
        #endif
      }

      //Put curves in the plots
      const std::size_t n_plots = m_plots.size();
      for (std::size_t i=0; i!=n_plots; ++i)
      {
        m_plots[i]->setAxisScale(QwtPlot::xBottom,0.0,n_timesteps);
        const std::vector<double> min_values
          = {
            *std::min_element(vs[(i*3)+0].begin(),vs[(i*3)+0].end()),
            *std::min_element(vs[(i*3)+1].begin(),vs[(i*3)+1].end()),
            *std::min_element(vs[(i*3)+2].begin(),vs[(i*3)+2].end())
          };
        const double min_value = *std::min_element(min_values.begin(),min_values.end());
        const std::vector<double> max_values
          = {
            *std::max_element(vs[(i*3)+0].begin(),vs[(i*3)+0].end()),
            *std::max_element(vs[(i*3)+1].begin(),vs[(i*3)+1].end()),
            *std::max_element(vs[(i*3)+2].begin(),vs[(i*3)+2].end())
          };
        const double max_value = *std::max_element(max_values.begin(),max_values.end());
        m_plots[i]->setAxisScale(
          QwtPlot::yLeft,
          min_value == max_value ? 0.0 : min_value,
          min_value == max_value ? 1.0 : max_value
        );
        m_plots[i]->replot();
      }
    }
  }
  catch (boost::bad_lexical_cast&)
  {
    const std::vector<double> v(1,0.0);
    {
      const std::size_t sz = m_curves.size();
      for (std::size_t i=0; i!=sz; ++i)
      {
        #ifdef _WIN32
        m_curves[i]->setData(new QwtPointArrayData(&v[0],&v[0],v.size()));
        #else
        m_curves[i]->setData(&v[0],&v[0],v.size());
        #endif
      }
    }
    {
      const std::size_t sz = m_plots.size();
      for (std::size_t i=0; i!=sz; ++i)
      {
        m_plots[i]->replot();
      }
    }
  }
}

const boost::numeric::ublas::matrix<double> QtMainDialog::ToMatrix(const QTableWidget * const table)
{
  assert(table);
  const int n_rows = table->rowCount();
  const int n_cols = table->columnCount();
  boost::numeric::ublas::matrix<double> v(n_rows,n_cols);
  for(int col=0;col!=n_cols;++col)
  {
    for(int row=0;row!=n_rows;++row)
    {
      const auto item = table->item(row,col);
      if (item)
      {
        const std::string text = item->text().toStdString();
        v(row,col) = boost::lexical_cast<double>(text);
      }
      else
      {
        v(row,col) = 0.0;
      }
    }
  }
  return v;
}

const boost::numeric::ublas::vector<double> QtMainDialog::ToVector(const QTableWidget * const table)
{
  assert(table);
  assert(table->columnCount() == 1);
  const int n_rows = table->rowCount();
  boost::numeric::ublas::vector<double> v(n_rows);
  for(int row=0;row!=n_rows;++row)
  {
    const auto item = table->item(row,0);
    if (item)
    {
      const std::string text = item->text().toStdString();
      v(row) = boost::lexical_cast<double>(text);
    }
    else
    {
      v(row) = 0.0;
    }
  }
  return v;

}

void QtMainDialog::on_box_n_states_valueChanged(int arg1)
{
  const int n = arg1;
  m_can_do_sim = false;

  //Use >= so that in constructor the plots, curves, etc are created
  if (n >= ui->table_states->rowCount())
  {
    //State is added

    //Update tables
    ui->table_states->setRowCount(n);
    QTableWidgetItem * const item = new QTableWidgetItem("?");
    ui->table_states->setItem(n - 1,0,item);

    while (n > static_cast<int>(m_plots.size()))
    {
      QwtPlot * const plot = new QwtPlot;
      plot->setAxisTitle(QwtPlot::xBottom,"Time");

      QwtPlotCurve * const curve_estimate = new QwtPlotCurve;
      curve_estimate->setTitle("Estimated");
      curve_estimate->attach(plot);
      curve_estimate->setStyle(QwtPlotCurve::Lines);
      curve_estimate->setPen(QPen(QColor(255,0,0)));
      QwtPlotCurve * const curve_measured = new QwtPlotCurve;
      curve_measured->setTitle("Measured");
      curve_measured->attach(plot);
      curve_measured->setStyle(QwtPlotCurve::Lines);
      curve_measured->setPen(QPen(QColor(0,255,0)));
      QwtPlotCurve * const curve_real = new QwtPlotCurve;
      curve_real->setTitle("Real");
      curve_real->attach(plot);
      curve_real->setStyle(QwtPlotCurve::Lines);
      curve_real->setPen(QPen(QColor(0,0,255)));
      m_curves.push_back(curve_estimate);
      m_curves.push_back(curve_measured);
      m_curves.push_back(curve_real);
      m_plots.push_back(plot);

      //Add grid
      { QwtPlotGrid * const grid = new QwtPlotGrid; grid->setPen(QPen(QColor(196,196,196))); grid->attach(plot); }
      //Add zoomer
      {
        new QwtPlotZoomer(plot->canvas());
      }
      ui->layout_plots->addWidget(plot);
    }
  }
  else
  {
    //State is removed
    ui->layout_plots->removeWidget(m_plots.back());
    delete m_plots.back();
    m_plots.pop_back();
    for (int i=0; i!=3; ++i)
    {
      //delete m_curves.back(); //Done by plot
      m_curves.pop_back();
    }
  }

  //Resize Matrices
  {
    const std::vector<QTableWidget *> v = CollectMatrices();
    const std::size_t sz = v.size();
    for (std::size_t i = 0; i!=sz; ++i)
    {
      QTableWidget * const table = v[i];
      table->setColumnCount(n);
      table->setRowCount(n);
    }
  }
  //Vectors
  {
    const std::vector<QTableWidget *> v = CollectVectors();
    const std::size_t sz = v.size();
    for (std::size_t i = 0; i!=sz; ++i)
    {
      QTableWidget * const table = v[i];
      table->setRowCount(n);
    }
  }
  UpdateLegends();
  m_can_do_sim = true;
  OnAnyChange();
}

void QtMainDialog::UpdateLegends()
{
  const std::vector<std::string> legend = GetLegend();
  const int n = static_cast<int>(legend.size());
  for (int i=0; i!=n; ++i)
  {
    m_plots[i]->setTitle(legend[i].c_str());
    m_plots[i]->setAxisTitle(QwtPlot::yLeft,legend[i].c_str());
  }
  //Matrices
  {
    const std::vector<QTableWidget *> v = CollectMatrices();
    const std::size_t sz = v.size();
    for (std::size_t i = 0; i!=sz; ++i)
    {
      QTableWidget * const table = v[i];
      assert(table->columnCount() == static_cast<int>(legend.size()));
      assert(table->rowCount() == static_cast<int>(legend.size()));
      for (int j = 0; j!=n; ++j)
      {
        {
          QTableWidgetItem * const item = new QTableWidgetItem;
          item->setText(legend[j].c_str());
          table->setVerticalHeaderItem(j,item);
        }
        {
          QTableWidgetItem * const item = new QTableWidgetItem;
          item->setText(legend[j].c_str());
          table->setHorizontalHeaderItem(j,item);
        }
      }
      table->resizeColumnsToContents();
      table->resizeRowsToContents();
      table->setFixedSize(
        table->horizontalHeader()->length() + 2 + table->verticalHeader()->width(),
        table->verticalHeader()->length() + 2 + table->horizontalHeader()->height());
    }
  }
  //Vectors
  {
    const std::vector<QTableWidget *> v = CollectVectors();
    const std::size_t sz = v.size();
    for (std::size_t i = 0; i!=sz; ++i)
    {
      QTableWidget * const table = v[i];
      assert(table->rowCount() == static_cast<int>(legend.size()));
      for (int j = 0; j!=n; ++j)
      {
        QTableWidgetItem * const item = new QTableWidgetItem;
        item->setText(legend[j].c_str());
        table->setVerticalHeaderItem(j,item);
      }
      table->resizeColumnsToContents();
      table->resizeRowsToContents();
      table->setFixedSize(
        table->horizontalHeader()->length() + 2 + table->verticalHeader()->width(),
        table->verticalHeader()->length() + 2 + table->horizontalHeader()->height());
    }
  }
}

 

 

 

 

 

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