"Hello world!" filter

Disclaimer: this article is suitable, in my opinion, primarily for beginners who do not understand anything about filtering, and the standard and adaptive Kalman filters are bewildering. Please treat her with this reservation.

Introduction

Very often, the information we receive may not correspond to reality, which is why its use in its pure form may be incorrect. Such a discrepancy is usually caused by certain noises, but most often when modeling, AWGN is taken as noise (additive white gaussian noise), which has a relatively stable amplitude. Filtering solves this problem.

However, there are a lot of filters. Despite their limitless variety, the standard is definitely Kalman filter. Unfortunately, for a person who has never worked with it, it can be quite difficult to decide on its implementation – at least because of the choice of input matrices.

That is why at the initial stages some points can be neglected and simplified Kalman filter to level α-β filter. If the reader has neither the time nor the inclination to advance on this topic, it is recommended to skip the next chapter and go straight to α-β filter.

Kalman filter

You can write a lot and for a very long time about the Kalman filter, but I will try to describe everything in thesis. At the same time, the main goal of this section is to show that it is not so easy to get into this topic right away, but to implement α-β filter much easier.

So let’s get started. Let the dynamic system be described by the following linear equations:

$z_k -vector\ measurements\ systems\ at\ step\ k$

$H - matrix\transition\dimension$

$v_k -vector\noise\measurement\at\step\k$

Then the state of the object will be evaluated as follows:

$x_{k+1}=Fx_k+w_k,$

$x_k -vector\ systems\ systems\ at\ step\ k$

$F - matrix\transition\state$

$w_k - vector\ noise\ process\ at\ step\ k$

At the same time, the noise And have covariance matrices And respectively. At the same time, the true values of these matrices to us unknownthat is, we will need to evaluate them ourselves.

If you are familiar with the word “covariance”, and your mathematical knowledge is at least somewhat in order, then this filter is quite possible to master by reading other articles on this excellent site, or by reading the relevant literature. The Kalman filter can be studied for a very, very long time, since it also has its own nuances, or you can stop at something that has been achieved – we will be content with the most trivial, but nevertheless quite effective.

α-β Kalman filter

Filter description:

Filtering allows us to get both the value itself at a given step , and the rate of its change, and in addition – extrapolation. Extrapolation is a prediction. That is, based on what we already have, we can assume what will happen next. All this can be used at your discretion.

So, for the filter to work, we need to determine the following coefficients:

$α = \frac{2(2 k − 1)}{k(k + 1)},\ β = \frac{6}{k(k + 1)}$

We will need them to filter the value and the rate at which it changes. It is important to understand that anything can act as this value, for example: the coordinate of the body along xBy y or by z, range, angle, speed, acceleration – whatever. Let’s look at the formulas:

$x_k=x_{k|k-1}+α(z_k-x_{k|k-1}),\\ \dot{x}_k=\dot{x}_{k|k-1}+\frac {β}{T_0}(z_k-x_{k|k-1})$

$z_k - measured \ value \ at \ step \ k$

$x_k - filtered\ value\ at\ step\ k$

$x_{k|k-1} - predicted\ value\ per\ step\ k\ from\ step\ {k-1}$

$\dot{x}_k - filtered \value\of\speed\in\step\k$

$\dot{x}_{k|k-1} - predicted\value\of\speed\per\step\k\s\step\k-1$

$T_0 - time\interval\between\measurements$

Extrapolation, that is, a forecast, is done as follows:

$x_{k|k-1}=x_{k-1}+T_0\dot{x}_k,\\ \dot{x}_{k|k-1}=\dot{x}_{k-1 }$

Leaving the filter in this form would, in my opinion, be a mistake, since the longer we filter, the more and the smaller the coefficients, which means that we trust the measurements less and more and more our forecasts, which is not always correct. The solution to this problem is to determine the quantity $k_{max}$ , upon reaching which we stop recalculating the coefficients α, β. You can take a value, for example, equal to 25 or 50.

The operation of the filtering algorithm by steps:

Since at first we don’t have enough information for the filter to work, initialization takes place in the first two steps:

Then everything goes as it should:

Later units of time we get new measurements ;
We calculate the coefficients α, β according to the corresponding formulas;
Filtering measurements and getting values And $\dot{x}_k$ ;
We make a prediction for the next step, that is, we calculate $x_{k|k-1}$ And $\dot{x}_{k|k-1}$ ;

Note that as soon as the value reaches the value $k_{max}$ we skip the second step and use the already calculated values of α, β coefficients.

For example, when

We get
$α=\frac{2(4 − 1)}{2(2 + 1)}=1,\ β= \frac{6}{2(2 + 1)}=1;$
$x_2=x_{2|1}+α(z_2-x_{2|1}),\ \dot{x}_2=\dot{x}_{2|1}+\frac{β}{T_0}( z_2-x_{2|1});$
$x_{3|2}=x_2+T_0\dot{x}_2,\ \dot{x}_{3|2}=\dot{x}_3;$

Implementation

In this article, all the steps of the algorithm are analyzed in sufficient detail, all the formulas are given, so it should not be difficult to implement the code, but I will still give a code fragment in C ++.

So the code should look something like this:

k=0;
T_0 = 5.0;

// Получаем z_k
void makeStep(double measuredValue) {
  k++;
  // Первые два шага фильтра нужны для его инициализации
  if(k == 1) {
    filteredValue = measuredValue;
    return;
  } else if(k == 2) {
    filteredVelocity = (measuredValue - filteredValue) / T_0;
    filteredValue = measuredValue;
    
    extrapolatedValue = filteredValue + (filteredVelocity * T_0);
    extrapolatedVelocity = filteredVelocity;
    return;
  }
  
  // Вычисляем коэффициенты фильтра
  alpha = (2.0 * (2.0 * k - 1.0)) / (k * (k + 1.0));
  beta = 6.0 / (k * (k + 1));
  
  // Фильтруем пришедшее значение
  filteredValue = extrapolatedValue + (alpha * (measuredValue - extrapolatedValue));
  filteredVelocity = extrapolatedVelocity + (beta / T_0 * (measuredValue - extrapolatedValue));

  // Делаем экстраполяцию, то есть прогнозируем значение на следующий шаг
  extrapolatedValue = filteredValue + (filteredVelocity * T_0);
  extrapolatedVelocity = filteredVelocity;
}

// P.S.: Как вы получите доступ к filteredValue,
// filteredVelocity, extrapolatedValue, extrapolatedVelocity -- дело Ваше.