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:

[x] Create a simulation program in Matlab with a basic Kalman Estimator.
[x] Translate Matlab code to C++ program and estimator using the C++17 standard.
[x] Write a supporting matrix library.
[x] Write a supporting fixed-point library.
[x] Write a supporting minimal standard C++ library.
[x] Install recent version of AVR GCC.
[x] Create Python script to compile C++ code for AVR.
[x] Create Python script to upload HEX file for AVR.
[x] Create Cmake toolchain for AVR.
[x] Create Cmake configuration scripts: toplevel, example, example/avr, example/desktop, test, time
[x] Create basic blink LED example:
- [x] Write blink LED code
- [x] Compile code to .hex
- [x] Upload code using avrdude and run it
[x] 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.
- [x] Write code to time
- [x] Compile code to .hex
- [x] Upload code using avrdude and run it
[x] Create code example with an interrupt-timed blinking LED (compile).
[x] Create code example reading the ADC.
[x] Create code example writing a pulse-width modulated DAC.
[x] 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.
[ ] ...

Relative Performance	F [kHz]	Type	Kalman gain	Optimization	Code size [B]
640	1600	Blink LED		-O2	174

0.18	0.473	double	updating	-Os	4,968
0.2	0.548	double	updating	-O2	5,492
0.8	2.1	double	fix on %chg	-Os	4,966
1.4	3.6	double	fix on %chg	-O2	5,490
0.7	1.7	fixed_point<int32_t>	updating	-Os	2.848
1	2.5	fixed_point<int32_t>	updating	-O2	2.490
4	9.9	fixed_point<int32_t>	fix on %chg	-Os	2.848
18	44.9	fixed_point<int32_t>	fix on %chg	-O2	2.490

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

A constant accelerated object met system and measurement noise

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