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)

IDE(s):

Project type:

C++ standard:

Compiler(s):

Libraries used:

 

 

 

 

 

Qt project file: CppKalmanFilterExample2.pro

 

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

win32 {
  INCLUDEPATH += E:/boost_1_50_0

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

}


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

HEADERS += \
    kalmanfilter.h \
    whitenoisesystem.h

 

 

 

 

 

kalmanfilter.h

 

#ifndef KALMANFILTER_H
#define KALMANFILTER_H

struct KalmanFilter
{
  ///Initialize the filter with a first measurent
  KalmanFilter(
    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; }

  private:

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

};

#endif // KALMANFILTER_H

 

 

 

 

 

kalmanfilter.cpp

 

#include "kalmanfilter.h"


KalmanFilter::KalmanFilter(
  const double first_x,
  const double first_p,
  const double measurement_noise,
  const double process_noise)
  : m_measurement_noise(measurement_noise),
    m_p(first_p),
    m_process_noise(process_noise),
    m_x(first_x)
{

}

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;

}

 

 

 

 

 

main.cpp

 

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

///Context:
///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
    k.SupplyMeasurement(z_measured);
    //Display what the filter predicts
    const double x_est_last = k.Predict();
    std::cout << x_real << "," << z_measured << "," << x_est_last << '\n';
  }
}

 

 

 

 

 

whitenoisesystem.h

 

#ifndef WHITENOISESYSTEM_H
#define WHITENOISESYSTEM_H

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

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

};

#endif // WHITENOISESYSTEM_H

 

 

 

 

 

whitenoisesystem.cpp

 

#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),
    m_stddev(stddev)
{

}

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