5 Kalman Filter

Principle

The Kalman filter, also called linear quadratic estimation (LQE), is an algorithm that uses a series of measurements to estimate unknown variables in the future. State estimation can, for example, be used to predict the placement of a robotic vacuum cleaner to avoid hitting walls or for creating a balancing robot.

The figure below shows the general principle of the Kalman filter. After initialization, the prediction and update phases happen when the algorithm runs. The update phase updates the variables based on the error it had from the prediction.

Example

This section will go through the example code on GitHub. The example we are using is done with a 3x3 matrix but can be changed for other examples if needed.

int main(void)
{
    SYSTEM_Initialize();


    PORTD.DIRSET = PIN6_bm;
	
	matrix_unittests();
	while(1){
		
		if(SEND_OVER_USART)
		{
			// Code that sends the data over USART.
			kalman_gravity_demo_usart();
		}else{
			// Regular code, without USART for easier cycle
			// measurement on PIN.
			kalman_gravity_demo();
		}
		
		kalman_gravity_demo_lambda();
	}
}

Kalman Gravity Demo

This demo calculates the gravity g based on the position S of a free-falling test body and is written by Markus Mayer. The source can be found at https://github.com/sunsided/kalman-clib.

The code uses three states (position, velocity, gravity), where gravity is the estimated acceleration based on the traveled distance, velocity and previous estimate of acceleration. As mentioned in the code, the formulas for this experiment are:

$S = S + v * T + g * 0.5 * T^{2}$

$v = v + g * T$

$g = g$

with:

S position in m
v velocity in m/s
T time, stepped in 1 s
g gravity, with an initial value of $6 \frac{m}{s^{2}}$ . This is a “bad estimation” which the Kalman filter shall improve.

The values for S are “noisy” positions, recorded with one second in between them.

The code prepares the initial values and iterates over the distances to predict the relative acceleration of the test body. The sequence is:

Predict the following distance.
Measure the actual following distance.
Calculate the error.
Update the Z matrix.

The Z matrix is further used in the kalman_correct() routine, which updates and corrects the variables, so the sequence can repeat.

When going through the sequence a couple of times, the calculated value for g quickly approaches a final value.

To measure the speed of the Kalman filter, connect an oscilloscope or a logic analyzer to PORT D - PIN 6 and measure the time when the pin is high. A more thorough explanation of how to use the Kalman filter is in the README.md of the code.

void kalman_gravity_demo()
{	
	PORTD.DIRSET = PIN6_bm;
	
	// initialize the filter
	PORTD.OUTSET = PIN6_bm;
	kalman_gravity_init();
	PORTD.OUTCLR = PIN6_bm;
	
	// fetch structures
	kalman_t *kf = &kalman_filter_gravity;
	kalman_measurement_t *kfm = &kalman_filter_gravity_measurement_position;

	matrix_t *x = kalman_get_state_vector(kf);
	matrix_t *z = kalman_get_measurement_vector(kfm);
	
	// filter!
	for (int i = 0; i < MEAS_COUNT; ++i)
	{
		PORTD.OUTSET = PIN6_bm;
		// prediction.
		kalman_predict(kf);
		// x[0] -> s_i -> estimate next position
		// x[1] -> v_i -> estimate next velocity
		// g[2] -> g_i -> estimate next gravity
		
		// measure ...
		matrix_data_t measurement = real_distance[i] + measurement_error[i];
		// reading position as float -> converting to uint8 array
		volatile uint8_t* measurement_b = (uint8_t*) &measurement;
		matrix_set(z, 0, 0, measurement);

		// update
		kalman_correct(kf, kfm);
		PORTD.OUTCLR = PIN6_bm;
		PORTD.DIRCLR = PIN6_bm;

	}
	

	// fetch estimated g
	matrix_data_t g_estimated = x->data[2];
	assert(g_estimated > 9 && g_estimated < 10);
}

To illustrate the continuous improvement of the estimates, set SEND_OVER_USART to TRUE in peripherals/usart.h. This will make sure to use the kalman_gravity_demo_usart(), which sends the data via USART.

Note: To get a more accurate cycles measurement of the algorithm, set SEND_OVER_USART to FALSE when doing cycle measurements.

Configure the Data Visualizer by selecting Load Workspace. Choose the configuration file data_visualizer.dvws.

The yellow line is the measurement of the position (i.e., sensor reading), the red line is the predicted position for the following measurement, and the purple line is the calculated gravity.

One new measurement is taken every second, giving feedback to the estimates of the next position and gravity g. The plot of gravity g starts with the initialization value of $6 \frac{m}{s^{2}}$ and improves the estimates towards $9.81 \frac{m}{s^{2}}$ .

Performance and Properties

Table 5-1. Cycle Times - Median Filter
Matrix Size	Initialization	One Estimation
A=3x3, B=null	1700	440

The Kalman filter is an advanced filter algorithm, and therefore, the long run-time of this algorithm is not unexpected. The time consumption further increases for larger matrix sizes. Smaller matrix sizes run faster.

Conclusion and Use Cases

This application note has given an overview of how to use this filter, provides typical cycle times of the filter, and walks us through an example to calculate the acceleration from position measurements.

A Kalman filters can be used in many situations where it is beneficial to know the next state based on previous input. Use cases for the Kalman filter are often found in mobility applications, such as balancing robots, vacuum cleaners, and even self-driving cars.

AN4515