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.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();
}
}
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:
When going through the sequence a couple of times, the calculated value for g quickly approaches a final value.
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);
}
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.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
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.
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.