Go back to Richel Bilderbeek's homepage.

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






(C++) Kalman filter example 2: single state, use of classes


Kalman filter example 2: single state, use of classes is an example to use a Kalman filter.







Technical facts


Application type(s)

Operating system(s) or programming environment(s)


Project type:

C++ standard:


Libraries used:






Qt project file: CppKalmanFilterExample2.pro


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

win32 {
  INCLUDEPATH += E:/boost_1_50_0

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


SOURCES += main.cpp \
    kalmanfilter.cpp \

    kalmanfilter.h \









struct KalmanFilter
  ///Initialize the filter with a first measurent
    const double first_x,
    const double first_p,
    const double measurement_noise,
    const double process_noise

  ///Give the filter a real measurement, so it will update itself
  void SupplyMeasurement(const double x);

  ///Let the filter predict
  double Predict() const { return m_x; }

  ///Let the filter predict
  double PredictCovariance() const { return m_p; }


  //R: Estimated measurement noise: How to estimate this?
  const double m_measurement_noise;

  ///The (current prediction of the) covariance
  double m_p;

  //Q: Process noise: How to estimate this?
  const double m_process_noise;

  ///The (current prediction of the) measurement
  double m_x;










#include "kalmanfilter.h"

  const double first_x,
  const double first_p,
  const double measurement_noise,
  const double process_noise)
  : m_measurement_noise(measurement_noise),


void KalmanFilter::SupplyMeasurement(const double x)
  /// 1/7) State prediction
  const double x_current = m_x;
  /// 2/7) Covariance prediction
  const double p_current = m_p + m_process_noise;
  /// 3/7) Innovation (y with a squiggle above it)
  const double z_measured = x; //x has noise in it
  const double innovation = z_measured - x_current;
  /// 4/7) Innovation covariance (S)
  const double innovation_covariance = p_current + m_measurement_noise;
  /// 5/7) Kalman gain (K)
  const double kalman_gain = p_current / innovation_covariance;
  /// 6/7) Update state prediction
  m_x = x_current + (kalman_gain * innovation);
  /// 7/7) Update covariance prediction
  m_p = (1.0 - kalman_gain) * p_current;









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

#include <iostream>
#include <boost/shared_ptr.hpp>

#include "kalmanfilter.h"
#include "whitenoisesystem.h"

///A constant DC voltage measured with a noisy multimeter
int main()
  const double x_real = 1.25; //Volts
  const double x_real_noise = 0.25; //Real measurement noise

  const double x_first_guess = 10.0; //Volts
  const double p_first_guess =  1.0; //Just a guess for the covariance
  const double measurement_noise = 0.1; //Estimated measurement noise
  const double process_noise = 0.0001; //Estimated process noise

  const WhiteNoiseSystem s(x_real,x_real_noise);

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

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









struct WhiteNoiseSystem

  WhiteNoiseSystem(const double real_value, const double stddev);

  ///Measure a value from this system with normally distributed noise
  double Measure() const;

  ///Peek what the real value is
  double PeekAtRealValue() const { return m_mean; }

  ///The real value of the system
  const double m_mean;

  ///The amount of noise in the system
  ///A noise of zero indicates a system that can be measured accurately to infinite precision
  const double m_stddev;

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










#include "whitenoisesystem.h"

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

WhiteNoiseSystem::WhiteNoiseSystem(const double real_value, const double stddev)
  : m_mean(real_value),


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;

double WhiteNoiseSystem::Measure() const
  return GetRandomNormal(m_mean,m_stddev);






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