kalman-estimator - a Kalman estimator in C++

Contents

• Example usage
• In a nutshell
• Basic Kalman estimator code
• Developing and testing the software
• Notes and references

Example usage

// Prepare variables...

// Create Kalman estimator:

kalman estim(
    dt          // time step
    , A         // system dynamics matrix: state-k-1 => state-k
    , B         // control input matrix: control => state
    , H         // measurement output matrix: state => measurement
    , Q         // process noise covariance
    , R         // measurement noise covariance
    , P         // initial estimate error covariance
    , xhat      // initial system state estimate
);

// Use the estimator, feed it a constant acceleration, system noise and a noisy measurement:

for ( int i = 0; i*dt < T; ++i )
{
    // Use a constant commanded acceleration of 1 [m/s^2]:
    const kalman::u_t u = {1};

    // Simulate the linear system:
    x = A * x + B * u + process_noise( dt, accelnoise );

    // Simulate the noisy measurement:
    auto z = H * x + meas_noise( measnoise );

    // Process a time-step:
    estim.update( u, z );
}

In a nutshell

Try and gain familiarity with the Kalman estimator. The intention is to implement a control system using a Kalman estimator in C++ on a small computer board like the Adafruit Pro Trinket and control the positioning of a spring–mass system with it.

Plan:

• Create a simulation program in Matlab with a basic Kalman Estimator.
• Translate Matlab code to C++ program and estimator using the C++17 standard.
• Write a supporting matrix library.
• Write a supporting fixed-point library.
• Write a supporting minimal standard C++ library.
• Install recent version of AVR GCC.
• Create Python script to compile C++ code for AVR.
• Create Python script to upload HEX file for AVR.
• Create Cmake toolchain for AVR.
• Create Cmake configuration scripts: toplevel, example, example/avr, example/desktop, test, time
• Create basic blink LED example:
  • Write blink LED code
  • Compile code to .hex
  • Upload code using avrdude and run it
• Time free-running loop on Trinket board for Kalman simulation with blink LED, varying floating/fixed point, updating/fixed Kalman gain and optimizations, see table 1. below.
  • Write code to time
  • Compile code to .hex
  • Upload code using avrdude and run it
• Create code example with an interrupt-timed blinking LED (compile).
• Create code example reading the ADC.
• Create code example writing a pulse-width modulated DAC.
• Create bitfield and register abstractions.
• Create atmega328 abstraction, see issue #1.
• Create code example with a timed ADC-DAC loop with a circular buffer.
• Design a simple setup to control via an Adafruit Pro Trinket (Arduino-like) board (spring–mass positioning).
• Create a demo application for the setup that implements a conventional PID controller.
• Create a demo application for the setup that implements a controller that uses the Kalman estimator.
• To reduce the computational load, implement (automatic) transitioning to a fixed Kalman gain after it has stabilized.
• Asses possible bottlenecks in the C++ code that may be easy to avoid.
• ...

Table 1. Relative performance for numeric type, fixing Kalman gain and compiler optimization, without ADC and DAC conversions.

Basic Kalman estimator code

void update( u_t const & u, z_t const & z )
{
	// --------------------------------------
	// 1. Predict (time update)
	
	// 1a: Project the state ahead:
	xhat = A * xhat + B * u;
	
	// 1b: Project the error covariance ahead:
	P = A * P * transposed(A) + Q;
	
	// --------------------------------------
	// 2. Correct (measurement update)
	
	// 2a: Compute the Kalman gain:
	K = P * transposed(H) * inverted(H * P * transposed(H) + R);
	
	// 2b: Update estimate with measurement:
	xhat = xhat + K * (z - H * xhat);
	
	// 2c: Update the error covariance:
	P = (I() - K * H) * P;
}

Graph for a simulation run with the resulting (estimated) position, (estimated) velocity, errors and Kalman gains.

Developing and testing the software

The software is developed using both AVR-GCC and a separate C++17 compiler on a personal computer. The software is developed and tested as a PC program using the lest test framework. In parallel, the AVR-GCC compiler is used to verify that what we develop as a C++ program is acceptable as an AVR program. The tool avrdude bundled with AVR-GCC is used to upload the program to the Pro Trinket.

Prerequisites

In what follows, it is expected that AVR GCC is available. If you want to compile and run the tests, a C++17 compiler such as GNU C++ or Visual C++ 2017 is needed.

(to be continued.)

Notes and References

Contents

• Kalman Estimator
• Matlab
• C++
• GNUC
• AVR
• Atmel
• Adafruit

Kalman Estimator

Various articles, video's, books to read up on the Kalman estimator.

[1] Wikipedia. Kalman filter.
[2] Greg Welch and Gary Bishop. An Introduction to the Kalman Filter (PDF).
[3] Greg Welch. Page on The Kalman Filter.
[4] Simon D. Levy. The Extended Kalman Filter: An Interactive Tutorial for Non-Experts.
[5] Bilgin Esme. Kalman Filter For Dummies - A mathematically challenged man's search for scientific wisdom.
[6] Mayitzin. Kalman Filter – A painless approach.
[7] Tucker McClure. How Kalman Filters Work, part 1, part 2, part 3 (Code on MathWorks).
[8] Tucker McClure. How Simulations Work.
[9] Demofox. Incremental Least Squares Curve Fitting (Contains C++ code).
[10] Kristian Sloth Lauszus. A practical approach to Kalman filter and how to implement it (Contains C code).
[11] Dan Simon. Optimal State Estimation: Kalman, H-infinity, and Nonlinear Approaches (Contains Matlab code from the book).
[12] iLectureOnline. Lectures in The Kalman Filter (42 videos of 6 minutes).

Fixed point

[13] Wikipedia. Fixed-point arithmetic.
[14] Rick Regan. Number of Bits in a Decimal Integer.

Matlab

[15] MathWorks. MATLAB for Deep Learning.
[16] GNU. GNU Octave - Scientific Programming Language (largely compatible with Matlab).

C++

[17] ISOCPP. Standard C++ Foundation.
[18] CppReference. The complete online reference for the C and C++ languages and standard libraries.
[19] Martin Moene. lest test framework.

GNUC

[20] GNUC. GNUC AVR Options.
[21] AVR-GCC. AVR-GCC 8.1.0 for Windows 32 and 64 bit. Contains section Upgrading the Arduino IDE.
[22] AVRDUDE. AVR Downloader/UploaDEr.

AVR

[23] Elliot Williams. AVR Programming - Learning to Write Software for Hardware (Code).
[24] Elliot Williams. Embed with Elliot: There is no Arduino "Language".

Atmel

[25] Atmel. Atmel Studio 7.
[26] Visual Micro. Arduino for Atmel Studio 7 (plug-in). [27] Atmel. Datasheet of ATmega328 Microcontroller (PDF).

Adafruit

[28] Adafruit. Pro Trinket.
[29] Adafruit. Introducing Pro Trinket.