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:

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

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