Now we are ready for the first simple example. In this example, we are going to estimate the state of the static system. The static system is a system that doesn't change its state over a reasonable period of time. For instance, the static system can be a tower, and the state is its height.

In this example, we are going to estimate the weight of the gold bar. We have unbiased scales, i.e. the scales measurements don't have a systematic error, but the measurements include random noise.

In this example, the system is the gold bar and the system's state is the weight of the gold bar. The system's dynamic model is constant, since we assume that the weight doesn't change over short period of time.

In order to estimate the system's state (i.e. the weight value), we can make multiple measurements and average them.

At the time \( N \), the estimate \( x_{N,N} \) would be the average of all previous measurements:

Notation:

\( x \) | is the true value of the weight |

\( z_{n} \) | is the measurement value of the weight at time \( n \) |

\( \hat{x}_{n,n} \) | is the estimate of \( x \) at time \( n \) (the estimate is made after taking the measurement \( z_{n} \) ) |

\( \hat{x}_{n,n-1} \) | is the previous estimate of \( x \) that was made at time \( n-1 \) (the estimate was made after taking the measurement \( z_{n-1} \)) |

\( \hat{x}_{n+1,n} \) | is the estimate of the future state (\( n+1 \) ) of \( x \). The estimate is made at the time \( n \), right after the measurement \( z_{n} \). In other words, \( x_{n,n+1} \) is a predicted state |

Note: In the literature, the caret over variable is used to indicate the estimated value.

The dynamic model in this example is constant, therefore \( x_{n+1,n}= x_{n,n} \).

In order to estimate \( \hat{x}_{N,N} \) we need to remember all historical measurements. Suppose that we don't have a pen and a paper in order to write down the measurements, we also can't rely on our memory and remember all historical measurements. We just want to use the previous estimate and add a small adjustment (in a real-life application, we want to save computer memory). We can do that with a small mathematical trick:

\[ = \frac{1}{N} \left( \sum _{n=1}^{N-1} \left( z_{n} \right) + z_{N} \right) = \]

\[ = \frac{1}{N} \sum _{n=1}^{N-1} \left( z_{n} \right) + \frac{1}{N} z_{N}= \]

\[ = \frac{1}{N}\frac{N-1}{N-1} \sum _{n=1}^{N-1} \left( z_{n} \right) + \frac{1}{N} z_{N}= \]

\[ = \frac{N-1}{N}\frac{1}{N-1} \sum _{n=1}^{N-1} \left( z_{n} \right) + \frac{1}{N} z_{N}= \]

\[ = \frac{N-1}{N}\hat{x}_{N,N-1}+ \frac{1}{N} z_{N}= \]

\[ = \hat{x}_{N,N-1}- \frac{1}{N}\hat{x}_{N,N-1}+ \frac{1}{N} z_{N}= \]

\[ = \hat{x}_{N,N-1}+ \frac{1}{N} \left( z_{N}- \hat{x}_{N,N-1} \right) \]

\( \hat{x}_{N,N-1} \) is the predicted state of \( x \) in the time \( N \), based on the measurement at time \( N-1 \). In other words, \( \hat{x}_{N,N-1} \) is the previous estimate.

The above equation is one of the five Kalman filter equations. It is called the State Update Equation. It means the following:

The factor \( \frac{1}{N} \) is specific for our example. We will discuss the important role of this factor later, but right now I would like to note that in the Kalman Filter, this factor is called Kalman Gain. It is denoted by \( K_{n} \). The subscript \( n \) indicates that the Kalman Gain can change with every iteration.

The discovery of the \( K_{n} \) was one of the Rudolf Kalman's major contributions.

Meanwhile, before we get into the guts of the Kalman Filter, we will use Greek letter \( \alpha _{n} \) instead of \( K_{n} \).

So, the State Update Equation will look like:

\[ \hat{x}_{n,n}= \hat{x}_{n,n-1}+ \alpha _{n} \left( z_{n}-\hat{x}_{n,n-1} \right) \]

The term \( \left( z_{n}- x_{n,n-1} \right) \) is a measurement residual, also called innovation. The innovation contains the new information.

In our example, \( \frac{1}{N} \) decreases as \( N \) increases. This means that in the beginning, we don't have enough information about the weight value, thus we base the estimation on the measurements. As we continue, each successive measurement has less weight in the estimation process, since \( \frac{1}{N} \) decreases. At some point, the contribution of the new measurements would become negligible.

Let continue with the example. Before we make the first measurement, we can guess (or rough estimate) the gold bar weight simply by reading the stamp on the gold bar. It is called an Initial Guess and it will be our first estimate.

As we will see later, the Kalman Filter requires the initial guess as a preset, and it can be very rough.

The following chart depicts the estimation algorithm that is used in this example.

Now we are ready to start the measurement and estimation process.

Our initial guess of the gold bar weight is 1000 gram. The initial guess is used only once for the filter initiation, thus it won't be required for the next iterations.

\[ \hat{x}_{0,0}=1000g \]

The weight of the gold bar is not supposed to change. Therefore, the dynamic model of the system is static. Our next state estimate (prediction) equals to the initialization:

\[ \hat{x}_{1,0} = \hat{x}_{0,0}=1000g \]

Making the weight measurement with the scales:

\[ z_{1}= 1030g \]

Calculating the gain. In our example \( \alpha _{n}= \frac{1}{n} \) , thus:

\[ \alpha _{1}= \frac{1}{1}=1 \]

Calculating the current estimate using the state update equation:

\[ \hat{x}_{1,1}= \hat{x}_{1,0}+ \alpha _{1} \left( z_{1}- \hat{x}_{1,0} \right) =1000+1 \left( 1030-1000 \right) =1030g \]

The dynamic model of the system is static, thus the weight of the gold bar is not supposed to change. Our next state estimate (prediction) equals to the current state estimate:

\[ \hat{x}_{2,1}= \hat{x}_{1,1}=1030g \]

After a unit time delay, the predicted estimate from the previous iteration becomes the previous estimate in the current iteration:

\[ \hat{x}_{2,1}=1030g \]

Making the second measurement of the weight:

\[ z_{2}= 989g \]

Calculating the gain:

\[ \alpha _{2}= \frac{1}{2} \]

Calculating the current estimate:

\[ \hat{x}_{2,2}= \hat{x}_{2,1}+ \alpha _{2} \left( z_{2}- \hat{x}_{2,1} \right) =1030+\frac{1}{2} \left( 989-1030 \right) =1009.5g \]

\[ \hat{x}_{3,2}= \hat{x}_{2,2}=1009.5g \]

\[ z_{3}= 1017g~~~~~~~~~~~~~~~~~~~~~~~~~~ \alpha _{3}= \frac{1}{3} \] \[ \hat{x}_{3,3}=~ 1009.5+\frac{1}{3} \left( 1017-1009.5 \right) =1012g \] \[ \hat{x}_{4,3}=1012g \]

\[ z_{4}= 1009g~~~~~~~~~~~~~~~~~~~~~~~~~~ \alpha _{4}= \frac{1}{4} \] \[ \hat{x}_{4,4}= 1012+\frac{1}{4} \left( 1009-1012 \right) =1011.25g \] \[ \hat{x}_{5,4}=1011.25g \]

\[ z_{5}= 1013g~~~~~~~~~~~~~~~~~~~~~~~~~~ \alpha _{5}= \frac{1}{5} \] \[ \hat{x}_{5,5}= 1011.25+\frac{1}{5} \left( 1013-1011.25 \right) =1011.6g \] \[ \hat{x}_{6,5}=1011.6g \]

\[ z_{6}= 979g~~~~~~~~~~~~~~~~~~~~~~~~~~ \alpha _{6}= \frac{1}{6} \] \[ \hat{x}_{6,6}= 1011.6+\frac{1}{6} \left( 979-1011.6 \right) =1006.17g \] \[ \hat{x}_{7,6}=1006.17g \]

\[ z_{7}=1008g~~~~~~~~~~~~~~~~~~~~~~~~~~ \alpha _{7}= \frac{1}{7} \] \[ \hat{x}_{7,7}= 1006.17+\frac{1}{7} \left( 1008-1006.17 \right) =1006.43g \] \[ \hat{x}_{8,7}=1006.43g \]

\[ z_{8}=1042g~~~~~~~~~~~~~~~~~~~~~~~~~~ \alpha _{8}= \frac{1}{8} \] \[ \hat{x}_{8,8}= 1006.43+\frac{1}{8} \left( 1042-1006.43 \right) =1010.87g \] \[ \hat{x}_{9,8}=1010.87g \]

\[ z_{9}=1012g~~~~~~~~~~~~~~~~~~~~~~~~~~ \alpha _{9}= \frac{1}{9} \] \[ \hat{x}_{9,9}= 1010.87+\frac{1}{9} \left( 1012-1010.87 \right) =1011g \] \[ \hat{x}_{10,9}=1011g \]

\[ z_{10}=1011g~~~~~~~~~~~~~~~~~~~~~~~~~~ \alpha _{10}= \frac{1}{10} \] \[ \hat{x}_{10,10}= 1011+\frac{1}{10} \left( 1011-1011 \right) =1011g \] \[ \hat{x}_{11,10}=1011g \]

We can stop here. The gain decreases with each measurement. Therefore, the contribution of each successive measurement is lower than the contribution of the previous measurement. We get quite close to a true weight which is 1010 gram. If we were making more measurements, we would get closer to the true value.

The following table summarizes our measurements and estimates, and the chart compares the measured values, the estimates and the true value.

\( n \) | 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 |
---|---|---|---|---|---|---|---|---|---|---|

\( \alpha _{n} \) | \( 1 \) | \( \frac{1}{2} \) | \( \frac{1}{3} \) | \( \frac{1}{4} \) | \( \frac{1}{5} \) | \( \frac{1}{6} \) | \( \frac{1}{7} \) | \( \frac{1}{8} \) | \( \frac{1}{9} \) | \( \frac{1}{10} \) |

\( z_{n} \) | 1030 | 989 | 1017 | 1009 | 1013 | 979 | 1008 | 1042 | 1012 | 1011 |

\( \hat{x}_{n,n} \) | 1030 | 1009.5 | 1012 | 1011.25 | 1011.6 | 1006.17 | 1006.43 | 1010.87 | 1011 | 1011 |

\( \hat{x}_{n+1,n} \) | 1030 | 1009.5 | 1012 | 1011.25 | 1011.6 | 1006.17 | 1006.43 | 1010.87 | 1011 | 1011 |

We can see that our estimation algorithm has a smoothing effect on the measurements, and it converges towards the true value.

In this example, we've developed a simple estimation algorithm for a static system. We have also derived the state update equation, which is one of the five Kalman Filter equations.

Now, it is time to examine a dynamic system that changes its state over the time. In this example, we are going to track the constant velocity aircraft in one dimension using the α-β filter.

Let us assume a one-dimensional world. We assume an aircraft that is moving radially away from the radar (or towards the radar). In the one-dimensional world, the angle to the radar is constant and the altitude of the aircraft is constant, as shown on the following figure.

\( x_{n} \) is representing the range to the aircraft at time \( n \) . The aircraft velocity is defined as the rate of change of the range with respect to time, thus the velocity is a derivative of the range:

\[ \dot{x}= v= \frac{dx}{dt} \]

The radar sends a track beam in the direction of the target with a constant rate. The track-to-track period is \( \Delta t \).

Assuming constant velocity, the system's dynamic model can be described by two equations of motion:

\[ x_{n+1}= x_{n}+ \Delta t\dot{x}_{n} \]
\[ \dot{x}_{n+1}= \dot{x}_{n} \]

According to the equations, the aircraft range at the next track cycle equals to the range at the current track cycle plus the target velocity multiplied by track-to-track period. In this example, we assume constant velocity. Therefore, the velocity at the next cycle equals to the velocity at the current cycle.

The above system of equations is called a State Extrapolation Equation (also called a Transition Equation or a Prediction Equation) and this is one of the five Kalman filter equations. This system of equations extrapolates the current state to the next state (prediction).

We have already used the State Extrapolation Equation in the previous example, where we assumed that the weight at the next state is equal to weight at the current state.

There is a general form of the State Extrapolation Equation in matrix notation, we will learn it later.

In this example, we will use the above set of equations that is specific for our case.

- State Update Equation
- State Extrapolation Equation

Now we are going to modify the State Update Equation for our example.

Let the radar track-to-track ( \( \Delta t \) ) period to be 5 seconds. Assume that at time \( n \) the estimated range of the UAV (Unmanned Aerial Vehicle) is 30,000m and the estimated UAV velocity is 40m/s.

Using the State Extrapolation Equations we can predict the target position at the time \( n + 1 \):

\[ \hat{x}_{n+1,n}= \hat{x}_{n,n}+ \Delta t\hat{\dot{x}}_{n,n}=30000+5\ast40=30200m \]

The target velocity at the time \( n + 1 \):

\[ \hat{\dot{x}}_{n+1,n}= \hat{\dot{x}}_{n,n}=40m/s \]

However, at time \( n \) the radar measures range ( \( z_{n} \) ) of 30110m and not 30200m as expected. There is a gap of 90m between the expected (predicted) range and the measured range. There are two possible reasons for this gap:

- The radar measurements are not precise
- The aircraft velocity has changed. The new aircraft velocity is: \( \frac{30110-30000}{5}=22m/s \)

Which of the two statements is true?

Let us write down the State Update Equation for the velocity:

The value of a factor \( \beta \) depends on the precision level of the radar. Suppose that the \( 1 \sigma \) precision of the radar is 20m. Most likely, the 90 meters gap between the predicted range and the measured range is resulted by the change in the aircraft velocity. In this case we set high value to the \( \beta \) factor. If we set \( \beta \) = 0.9, then the estimated velocity would be:

\[ \hat{\dot{x}}_{n,n}= \hat{\dot{x}}_{n,n-1}+ \beta \left( \frac{z_{n}-\hat{x}_{n,n-1}}{ \Delta t} \right) =40+0.9 \left( \frac{30110-30200}{5} \right) =23.8m/s \]

Suppose that the \( 1 \sigma \) precision of the radar is 150m. Most likely, the 90 meters gap is resulted by the radar measurement error. In this case, we set low value to the \( \beta \) factor. If we set \( \beta \) = 0.1, then the estimated velocity would be:

\[ \hat{\dot{x}}_{n,n}= \hat{\dot{x}}_{n,n-1}+ \beta \left( \frac{z_{n}-\hat{x}_{n,n-1}}{ \Delta t} \right) =40+0.1 \left( \frac{30110-30200}{5} \right) =38.2m/s \]

The State Update Equation for the aircraft position, is similar to the equation that was derived in the previous example:

\[ \hat{x}_{n,n}=~ \hat{x}_{n,n-1}+ \alpha \left( z_{n}- \hat{x}_{n,n-1} \right) \]

On contrary to the pervious example, where the \( \alpha \) factor is recalculated in each iteration ( \( \alpha _{n}= \frac{1}{N} \) ), in this example the \( \alpha \) factor is constant.

The magnitude of the \( \alpha \) factor depends on the radar measurement precision. For high precision radar we will choose high \( \alpha \) , giving a high weight to the measurements. If \( \alpha =1 \) , then the estimated range equals to the measured range:

\[ \hat{x}_{n,n}=~ \hat{x}_{n,n-1}+ 1 \left( z_{n}- \hat{x}_{n,n-1} \right) = z_{n} \]

If \( \alpha =0 \) , then the measurement has no meaning:

\[ \hat{x}_{n,n}=~ \hat{x}_{n,n-1}+ 0 \left( z_{n}- \hat{x}_{n,n-1} \right) = x_{n,n-1} \]

So, we have derived a system of equations that composes the Update State Equation for the radar tracker. They are also called \( \alpha - \beta \) track update equations or \( \alpha - \beta \) track filtering equations.

The Update State Equation for position:

\[ \hat{x}_{n,n}=~ \hat{x}_{n,n-1}+ \alpha \left( z_{n}- \hat{x}_{n,n-1} \right) \]
The Update State Equation for velocity:

The following chart depicts the estimation algorithm that is used in this example.

On contrary to the pervious example, the Gain values \( \alpha \) and \( \beta \) are given for this example. In the Kalman Filter the \( \alpha \) and \( \beta \) are replaced by Kalman Gain that is calculated at each iteration, but we will learn it later.

Now we are ready to start the numerical example.

Given the aircraft that is moving radially away from the radar (or towards the radar) in one-dimensional world.

The \( \alpha - \beta \) filter parameters are:

- \( \alpha =0.2 \)
- \( \beta =0.1 \)

The track-to-track period is 5 seconds.

The initial conditions for the time \( n=0 \) are given:

\[ \hat{x}_{0,0}=30000m \] \[ \hat{\dot{x}}_{0,0}=40m/s \]

The initial guess shall be extrapolated to the first cycle ( \( n=1 \) ) using the State Extrapolation Equations:

\[ \hat{x}_{n+1,n}= \hat{x}_{n,n}+ \Delta t\hat{\dot{x}}_{n,n} \rightarrow \hat{x}_{1,0}= \hat{x}_{0,0}+ \Delta t\hat{\dot{x}}_{0,0} =30000+5 \times 40=30200m \] \[ \hat{\dot{x}}_{n+1,n}= \hat{\dot{x}}_{n,n} \rightarrow \hat{\dot{x}}_{1,0}= \hat{\dot{x}}_{0,0} =40m/s \]

In the first cycle ( \( n=1 \) ), the initial guess is the previous estimate:

\[ \hat{x}_{n,n-1}=~ \hat{x}_{1,0}=30200m \] \[ \hat{\dot{x}}_{n,n-1}=~ \hat{\dot{x}}_{1,0}=40m/s \]

The radar measures the range to the aircraft:

\[ z_{1}= 30110m \]

Calculating the current estimate using the State Update Equation:

\[ \hat{x}_{1,1}=~ \hat{x}_{1,0}+ \alpha \left( z_{1}- \hat{x}_{1,0} \right) =30200+0.2 \left( 30110-30200 \right) =30182m \] \[ \hat{\dot{x}}_{1,1}= \hat{\dot{x}}_{1,0}+ \beta \left( \frac{z_{1}-\hat{x}_{1,0}}{ \Delta t} \right) = 40+0.1 \left( \frac{30110-30200}{5} \right) =38.2m/s \]

Calculating the next state estimate using the State Extrapolation Equations:

\[ \hat{x}_{2,1}= \hat{x}_{1,1}+ \Delta t\hat{\dot{x}}_{1,1} =30182+5 \times 38.2=30373m \] \[ \hat{\dot{x}}_{2,1}= \hat{\dot{x}}_{1,1} =38.2m/s \]

After a unit time delay, the predicted estimate from the previous iteration becomes a previous estimate in the current iteration:

\[ \hat{x}_{2,1}=30373m \] \[ \hat{\dot{x}}_{2,1}= 38.2m/s \]

The radar measures the range to the aircraft:

\[ z_{2}= 30265m \]

Calculating the current estimate using the state update equation:

\[ \hat{x}_{2,2}=~ \hat{x}_{2,1}+ \alpha \left( z_{2}- \hat{x}_{2,1} \right) =30373+0.2 \left( 30265-30373 \right) =30351.4m \] \[ \hat{\dot{x}}_{2,2}= \hat{\dot{x}}_{2,1}+ \beta \left( \frac{z_{2}-\hat{x}_{2,1}}{ \Delta t} \right) = 38.2+0.1 \left( \frac{30265-30373}{5} \right) =36m/s \]

Calculating the next state estimate using the State Extrapolation Equations:

\[ \hat{x}_{3,2}= \hat{x}_{2,2}+ \Delta t\hat{\dot{x}}_{2,2} =30351.4+5 \times 36=30531.6m \] \[ \hat{\dot{x}}_{3,2}= \hat{\dot{x}}_{2,2} =36m/s \]

\[ z_{3}= 30740 \] \[ \hat{x}_{3,3}=~ 30531.6+0.2 \left( 30740~ -30531.6 \right) =30573.3m \] \[ \hat{\dot{x}}_{3,3}= 36+0.1 \left( \frac{30740~ -30531.6}{5} \right) =40.2m/s \] \[ \hat{x}_{4,3}= 30573.3+5 \times 40.2=30774.3m \] \[ \hat{\dot{x}}_{4,3}= 40.2m/s \]

\[ z_{4}= 30750 \] \[ \hat{x}_{4,4}=~ 30774.3+0.2 \left( 30750-30774.3 \right) =30769.5m \] \[ \hat{\dot{x}}_{4,4}= 40.2+0.1 \left( \frac{30750-30774.3}{5} \right) =39.7m/s \] \[ \hat{x}_{5,4}= 30769.5+5 \times 39.7=30968.1m \] \[ \hat{\dot{x}}_{5,4}= 39.7m/s \]

\[ z_{5}= 31135 \] \[ \hat{x}_{5,5}=~ 30968.1+0.2 \left( 31135 -30968.1 \right) =31001.5m \] \[ \hat{\dot{x}}_{5,5}= 39.7+0.1 \left( \frac{31135 -31216.8}{5} \right) =43.1m/s \] \[ \hat{x}_{6,5}= 31001.5+5 \times 43.1=31216.8m \] \[ \hat{\dot{x}}_{6,5}= 43.1m/s \]

\[ z_{6}= 31015 \] \[ \hat{x}_{6,6}=~ 31216.8+0.2 \left( 31015 -31216.8 \right) =31176.4m \] \[ \hat{\dot{x}}_{6,6}= 43.1+0.1 \left( \frac{31015 -31216.8}{5} \right) =39m/s \] \[ \hat{x}_{7,6}= 31176.4+5 \times 39=31371.5m \] \[ \hat{\dot{x}}_{7,6}= 39m/s \]

\[ z_{7}= 31180 \] \[ \hat{x}_{7,7}=~ 31371.5+0.2 \left( 31180 -31371.5 \right) =31333.2m \] \[ \hat{\dot{x}}_{7,7}= 39+0.1 \left( \frac{31180 -31371.5}{5} \right) =35.2m/s \] \[ \hat{x}_{8,7}= 31333.2+5 \times 35.2=31509.2m \] \[ \hat{\dot{x}}_{8,7}= 35.2m/s \]

\[ z_{8}= 31610 \] \[ \hat{x}_{8,8}=~ 31509.2+0.2 \left( 31610 -31509.2 \right) =31529.4m \] \[ \hat{\dot{x}}_{8,8}= 35.2+0.1 \left( \frac{31610 -31509.2}{5} \right) =37.2m/s \] \[ \hat{x}_{9,8}= 31529.4+5 \times 37.2=31715.4m \] \[ \hat{\dot{x}}_{9,8}= 37.2m/s \]

\[ z_{9}= 31960 \] \[ \hat{x}_{9,9}=~ 31715.4+0.2 \left( 31960 -31715.4 \right) =31764.3m \] \[ \hat{\dot{x}}_{9,9}= 37.2+0.1 \left( \frac{31960 -31715.4}{5} \right) =42.1m/s \] \[ \hat{x}_{10,9}= 31764.3+5 \times 42.1=31974.8m \] \[ \hat{\dot{x}}_{10,9}=42.1m/s \]

\[ z_{10}= 31865 \] \[ \hat{x}_{10,10}=~ 31974.8+0.2 \left( 31865 -31974.8 \right) =31952.9m \] \[ \hat{\dot{x}}_{10,10}= 42.1+0.1 \left( \frac{31865 -31974.8}{5} \right) =39.9m/s \] \[ \hat{x}_{11,10}= 31952.9+5 \times 39.9=32152.4m \] \[ \hat{\dot{x}}_{11,10}= 39.9m/s \]

The following table summarizes our measurements and estimates.

\( n \) | 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 |
---|---|---|---|---|---|---|---|---|---|---|

\( z_{n} \) | 30110 | 30265 | 30740 | 30750 | 31135 | 31015 | 31180 | 31610 | 31960 | 31865 |

\( \hat{x}_{n,n} \) | 30182 | 30351.4 | 30573.3 | 30769.5 | 31001.5 | 31176.4 | 31333.2 | 31529.4 | 31764.3 | 31952.9 |

\( \dot{\hat{x}}_{n,n} \) | 38.2 | 36 | 40.2 | 39.7 | 43.1 | 39 | 35.2 | 37.2 | 42.1 | 39.9 |

\( \hat{x}_{n+1,n} \) | 30373 | 30531.6 | 30774.3 | 30968.1 | 31216.8 | 31371.5 | 31509.2 | 31715.4 | 31974.8 | 32152.4 |

\( \dot{\hat{x}}_{n+1,n} \) | 38.2 | 36.0 | 40.2 | 39.7 | 43.1 | 39 | 35.2 | 37.2 | 42.1 | 39.9 |

The following chart compares the true value, measured values and estimates.

We can see that our estimation algorithm has a smoothing effect on the measurements, and it converges towards the true value.

The following chart depicts the true value, measured values and estimates for \( \alpha = 0.8 \) and \( \beta = 0.5 \).

The "smoothing" degree of this filter is much lower. The "current estimate" is very close to the measured values and predicted estimate errors are quite big.

So shall we always choose low values for \( \alpha \) and \( \beta \) ?

The answer is NO. The value of \( \alpha \) and \( \beta \) shall depend on the measurement precision. If we use very precise equipment, like the laser radar, we would prefer high \( \alpha \) and \( \beta \) that follow measurements. In this case, the filter would quickly response on velocity change of the target. On the other side, if measurement precision is low, we would prefer low \( \alpha \) and \( \beta \) . In this case, the filter will smooth the uncertainty (errors) in the measurements. However, the filter reaction to the target velocity changes will be much slower.

Since the \( \alpha \) and the \( \beta \) calculation is an important topic, we will discuss it in more detail later.

In this example we've derived the \( \alpha - \beta \) filter state update equation. We've also learned the State Extrapolation Equation. We've developed an estimation algorithm for one-dimensional dynamic system based on the \( \alpha - \beta \) filter and solved a numerical example for the constant velocity target.

In this example, we are going to track the aircraft that is moving with constant acceleration with the \( \alpha - \beta \) filter that was already explained in the previous example.

In the previous example, we've tracked the UAV that is moving at constant velocity of 40m/s. The following chart depicts the target range and velocity vs. time.

As you can see, the range function is linear.

Now let's examine a fighter aircraft. This aircraft moves with the constant velocity of 50m/s for 15 seconds. Then the aircraft accelerates with constant acceleration of 8 m/s2 for another 35 seconds.

The following chart depicts the target range, velocity and acceleration vs. time.

As you can see from the chart, the aircraft velocity is constant for the first 15 seconds and then grows linearly. The range grows linearly for the first 15 seconds and then grows quadratically.

We are going to track this aircraft with the α-β filter that was used in the previous example.

The numerical example

Given the aircraft that is moving radially toward the radar (or away from) radar in the one-dimensional world.

The \( \alpha - \beta \) filter parameters are:

- \( \alpha =0.2 \)
- \( \beta =0.1 \)

The track-to-track period is 5 seconds.

The initial conditions for the time \( n=0 \) are given:

\[ \hat{x}_{0,0}=30000m \] \[ \hat{\dot{x}}_{0,0}=50m/s \]

The initial guess shall be extrapolated to the first cycle ( \( n=1 \) ) using the State Extrapolation Equations:

\[ \hat{x}_{n+1,n}= \hat{x}_{n,n}+ \Delta t\hat{\dot{x}}_{n,n} \rightarrow \hat{x}_{1,0}= \hat{x}_{0,0}+ \Delta t\hat{\dot{x}}_{0,0} =30000+5 \times 50=30250m \] \[ \hat{\dot{x}}_{n+1,n}= \hat{\dot{x}}_{n,n} \rightarrow \hat{\dot{x}}_{1,0}= \hat{\dot{x}}_{0,0} =50m/s \]

All filter iterations are summarized in the next table:

\( n \) | \( z_{n} \) | Current state estimates ( \( \hat{x}_{n,n} \), \( \hat{\dot{x}}_{n,n} \) ) | Prediction ( \( \hat{x}_{n+1,n} \), \( \hat{\dot{x}}_{n+1,n} \) ) |
---|---|---|---|

1 | \( 30160m \) | \[ \hat{x}_{1,1}=~ 30250+0.2 \left( 30160- 30250 \right) =30232m \] \[ \hat{\dot{x}}_{1,1}= 50+0.1 \left( \frac{30160 -30250}{5} \right) =48.2m/s \] | \[ \hat{x}_{2,1}= 30232+5 \times 48.2=30473m \] \[ \hat{\dot{x}}_{2,1}= 48.2m/s \] |

2 | \( 30365m \) | \[ \hat{x}_{2,2}=~ 30473+0.2 \left( 30365 -30473 \right) =30451.1m \] \[ \hat{\dot{x}}_{2,2}= 48.2+0.1 \left( \frac{30365 -30473}{5} \right) =46m/s \] | \[ \hat{x}_{3,2}= 30451.1+5 \times 46=30681.6m \] \[ \hat{\dot{x}}_{3,2}= 46m/s \] |

3 | \( 30890m \) | \[ \hat{x}_{3,3}=~ 30681.6+0.2 \left( 30890 -30681.6 \right) =30723.3m \] \[ \hat{\dot{x}}_{3,3}= 46+0.1 \left( \frac{30890 -30681.6}{5} \right) =50.2m/s \] | \[ \hat{x}_{4,3}= 30723.3+5 \times 50.2=30974.3m \] \[ \hat{\dot{x}}_{4,3}= 50.2m/s \] |

4 | \( 31050m \) | \[ \hat{x}_{4,4}=~ 30974.3+0.2 \left( 31050 -30974.3 \right) =30989.5m \] \[ \hat{\dot{x}}_{4,4}= 50.2+0.1 \left( \frac{31050 -30974.3}{5} \right) =51.7m/s \] | \[ \hat{x}_{5,4}= 30989.5+5 \times 51.7=31248.1m \] \[ \hat{\dot{x}}_{5,4}= 51.7m/s \] |

5 | \( 31785m \) | \[ \hat{x}_{5,5}=~ 31248.1+0.2 \left( 31785 -31248.1 \right) =31355.5m \] \[ \hat{\dot{x}}_{5,5}= 51.7+0.1 \left( \frac{31785 -31248.1}{5} \right) =62.5m/s \] | \[ \hat{x}_{6,5}= 31355.5+5 \times 62.5=31667.8m \] \[ \hat{\dot{x}}_{6,5}= 62.5m/s \] |

6 | \( 32215m \) | \[ \hat{x}_{6,6}=~ 31667.8+0.2 \left( 32215 -31667.8 \right) =31777.2m \] \[ \hat{\dot{x}}_{6,6}= 62.5+0.1 \left( \frac{32215 -31667.8}{5} \right) =73.4m/s \] | \[ \hat{x}_{7,6}= 31777.2+5 \times 73.4=32144.2m \] \[ \hat{\dot{x}}_{7,6}= 73.4m/s \] |

7 | \( 33130m \) | \[ \hat{x}_{7,7}=~ 32144.2+0.2 \left( 33130 -32144.2 \right) =32341.4m \] \[ \hat{\dot{x}}_{7,7}= 73.4+0.1 \left( \frac{33130 -32144.2}{5} \right) =93.1m/s \] | \[ \hat{x}_{8,7}= 32341.4+5 \times 93.1=32807m \] \[ \hat{\dot{x}}_{8,7}= 93.1m/s \] |

8 | \( 34510m \) | \[ \hat{x}_{8,8}=~ 32807+0.2 \left( 34510 -32807 \right) =33147.6m \] \[ \hat{\dot{x}}_{8,8}= 93.1+0.1 \left( \frac{34510 -32807}{5} \right) =127.2m/s \] | \[ \hat{x}_{9,8}= 33147.6+5 \times 127.2=33783.5m \] \[ \hat{\dot{x}}_{9,8}= 127.2m/s \] |

9 | \( 36010m \) | \[ \hat{x}_{9,9}=~ 33783.5+0.2 \left( 36010 -33783.5 \right) =34228.8m \] \[ \hat{\dot{x}}_{9,9}= 127.2+0.1 \left( \frac{36010 -33783.5}{5} \right) =171.7m/s \] | \[ \hat{x}_{10,9}= 34228.8+5 \times 171.7=35087.4m \] \[ \hat{\dot{x}}_{10,9}= 171.7m/s \] |

10 | \( 37265m \) | \[ \hat{x}_{10,10}=~ 35087.4+0.2 \left( 37265 -35087.4 \right) =35522.9m \] \[ \hat{\dot{x}}_{10,10}= 171.7+0.1 \left( \frac{37265 -35087.4}{5} \right) =215.3m/s \] | \[ \hat{x}_{11,10}= 35522.9+5 \times 215.3=36559.2m \] \[ \hat{\dot{x}}_{11,10}= 215.3m/s \] |

The following charts compare the true value, measured values and estimates for the range and velocity for the first 75 seconds.

You can see a constant gap between the true or measured values and the estimated values. The gap is called a lag error. Other common names for the lag error are:

- Dynamic error
- Systematic error
- Bias error
- Truncation error

In this example, we've examined the lag error that is caused by constant acceleration.

In this example, we are going to track the aircraft that is moving with constant acceleration with the \( \alpha -\beta -\gamma \) filter.

The \( \alpha -\beta -\gamma \) filter (sometimes called g-h-k filter) considers target acceleration, thus the State Extrapolation Equations become:

Where \( \ddot{x}_{n} \) is acceleration (the second derivative of \( x \) )

The State Update Equations become:

Let's take the scenario from the previous example: the aircraft that moves with the constant velocity of 50m/s for 15 seconds. Then the aircraft accelerates with constant acceleration of 8 m/s^{2} for another 35 seconds.

The \( \alpha \) - \( \beta \) filter parameters are:

- \( \alpha =0.5 \)
- \( \beta =0.4 \)
- \( \gamma =0.1 \)

The track-to-track period is 5 seconds.

The initial conditions for the time \( n=0 \) are given:

\[ \hat{x}_{0,0}=30000m \] \[ \hat{\dot{x}}_{0,0}=50m/s \] \[ \hat{\ddot{x}}_{0,0}=0m/s^{2} \]

The initial guess shall be extrapolated to the first cycle ( \( n=1 \) ) using the State Extrapolation Equations:

\[ \hat{x}_{n+1,n}= \hat{x}_{n,n}+ \hat{\dot{x}}_{n,n} \Delta t+ \hat{\ddot{x}}_{n,n}\frac{ \Delta t^{2}}{2}~~ \rightarrow \hat{x}_{1,0}= \hat{x}_{0,0}+ \hat{\dot{x}}_{0,0} \Delta t+ \hat{\ddot{x}}_{0,0}\frac{ \Delta t^{2}}{2}= 30000+ 50 \times 5+ 0 \times \frac{5^{2}}{2}=30250m \] \[ \hat{\dot{x}}_{n+1,n}= \hat{\dot{x}}_{n,n}+ \hat{\ddot{x}}_{n,n} \Delta t~ \rightarrow \hat{\dot{x}}_{1,0}= \hat{\dot{x}}_{0,0} + \hat{\ddot{x}}_{0,0} \Delta t =50+0 \times 5=50m/s \] \[ \hat{\ddot{x}}_{n+1,n}= \hat{\ddot{x}}_{n,n} \rightarrow ~ \hat{\ddot{x}}_{1,0}= \hat{\ddot{x}}_{0,0}= 0m/s^{2} \]

In the first cycle ( \( n=1 \) ), the initial guess is the previous estimate:

\[ \hat{x}_{n,n-1}=~ \hat{x}_{1,0}=30250m \] \[ \hat{\dot{x}}_{n,n-1}=~ \hat{\dot{x}}_{1,0}=50m/s \] \[ \hat{\ddot{x}}_{n,n-1}= \hat{\ddot{x}}_{1,0}= 0m/s^{2} \]

The radar measures the range to the aircraft:

\[ z_{1}= 30160m \]

Calculating the current estimate using the state update equation:

\[ \hat{x}_{1,1}=~ \hat{x}_{1,0}+ \alpha \left( z_{1}- \hat{x}_{1,0} \right) =30250+0.5 \left( 30160-30250 \right) =30205m \] \[ \hat{\dot{x}}_{1,1}= \hat{\dot{x}}_{1,0}+ \beta \left( \frac{z_{1}-\hat{x}_{1,0}}{ \Delta t} \right) = 50+0.4 \left( \frac{30160-30250}{5} \right) =42.8m/s \] \[ \hat{\ddot{x}}_{1,1}= \hat{\ddot{x}}_{1,0}+ \gamma \left( \frac{z_{1}-\hat{x}_{1,0}}{0.5 \Delta t^{2}} \right) = 0+0.1 \left( \frac{30160-30250}{0.5 \times 5^{2}} \right) =-0.7m/s^{2} \]

Calculating the next state estimate using the State Extrapolation Equations:

\[ \hat{x}_{2,1}= \hat{x}_{1,1}+ \hat{\dot{x}}_{1,1} \Delta t+ \hat{\ddot{x}}_{1,1}\frac{ \Delta t^{2}}{2}= 30205+ 42.8 \times 5+ \left( -0.7 \right) \times \frac{5^{2}}{2}=30410m \] \[ \hat{\dot{x}}_{2,1}=\hat{\dot{x}}_{1,1} + \hat{\ddot{x}}_{1,1} \Delta t =42.8+ \left( -0.7 \right) \times 5=39.2m/s \] \[ \hat{\ddot{x}}_{2,1}= \hat{\ddot{x}}_{1,1}= -0.7m/s^{2} \]

After a unit time delay, the predicted estimate from the foregoing iteration becomes a previous estimate in the current iteration.

\[ \hat{x}_{2,1}=30410m \] \[ \hat{\dot{x}}_{2,1}= 39.2m/s \] \[ \hat{\ddot{x}}_{2,1}= -0.7m/s^{2} \]

The radar measures the range to the aircraft:

\[ z_{2}= 30365m \]

Calculating the current estimate using the state update equation:

\[ \hat{x}_{2,2}=~ \hat{x}_{2,1}+ \alpha \left( z_{2}- \hat{x}_{2,1} \right) =30410+0.5 \left( 30365-30410 \right) =30387.5m \] \[ \hat{\dot{x}}_{2,2}= \hat{\dot{x}}_{2,1}+ \beta \left( \frac{z_{2}-\hat{x}_{2,1}}{ \Delta t} \right) = 39.2+0.4 \left( \frac{30365-30410}{5} \right) =35.6m/s \] \[ \hat{\ddot{x}}_{2,2}= \hat{\ddot{x}}_{2,1}+ \gamma \left( \frac{z_{2}-\hat{x}_{2,1}}{0.5 \Delta t^{2}} \right) = -0.7+0.1 \left( \frac{30365-30410}{0.5 \times 5^{2}} \right) =-1.1m/s^{2} \]

Calculating the next state estimate using the State Extrapolation Equations:

\[ \hat{x}_{3,2}= \hat{x}_{2,2}+ \hat{\dot{x}}_{2,2} \Delta t+ \hat{\ddot{x}}_{2,2}\frac{ \Delta t^{2}}{2}= 30387.5+ 35.6 \times 5+ \left( -1.1 \right) \times \frac{5^{2}}{2}=30552m \] \[ \hat{\dot{x}}_{3,2}=\hat{\dot{x}}_{2,2} + \hat{\ddot{x}}_{2,2} \Delta t =35.6+ \left( -1.1 \right) \times 5=30.2m/s \] \[ \hat{\ddot{x}}_{3,2}= \hat{\ddot{x}}_{2,2}= -1.1m/s^{2} \]

The calculations for the next iterations are summarized in the next table:

\( n \) | \( z_{n} \) | Current state estimates ( \( \hat{x}_{n,n} \), \( \hat{\dot{x}}_{n,n} \), \( \hat{\ddot{x}}_{n,n} \) ) | Prediction ( \( \hat{x}_{n+1,n} \), \( \hat{\dot{x}}_{n+1,n} \), \( \hat{\ddot{x}}_{n+1,n} \) ) |
---|---|---|---|

3 | \( 30890m \) | \[ \hat{x}_{3,3}=~ 30552+0.5 \left( 30890 -30552 \right) =30721m \] \[ \hat{\dot{x}}_{3,3}= 35.6+0.4 \left( \frac{30890 -30552}{5} \right) =57.2m/s \] \[ \hat{\ddot{x}}_{3,3}=~ -1.1+0.1 \left( \frac{30890 -30552}{0.5 \times 5^{2}} \right) =1.6m/s^{2} \] | \[ \hat{x}_{4,3}= 30721+5 \times 57.2 + 1.6 \times \frac{5^{2}}{2}=31027.5m \] \[ \hat{\dot{x}}_{4,3}= 57.2 + 1.6 \times 5=65.4m/s \] \[ \hat{\ddot{x}}_{4,3}= 1.6m/s^{2} \] |

4 | \( 31050m \) | \[ \hat{x}_{4,4}=~ 31027.5+0.5 \left( 31050 -31027.5 \right) = 31038.8m \] \[ \hat{\dot{x}}_{4,4}= 65.4+0.4 \left( \frac{31050 -31027.5}{5} \right) = 67.2m/s \] \[ \hat{\ddot{x}}_{4,4}=~ 1.6+0.1 \left( \frac{31050 -31027.5}{0.5 \times 5^{2}} \right) = 1.8m/s^{2} \] | \[ \hat{x}_{5,4}= 31038.8+5 \times 67.2 + 1.8 \times \frac{5^{2}}{2}=31397.1m \] \[ \hat{\dot{x}}_{5,4}= 67.2 + 1.8 \times 5=76.2m/s \] \[ \hat{\ddot{x}}_{4,3}= 1.8m/s^{2} \] |

5 | \( 31785m \) | \[ \hat{x}_{5,5}=~ 31397.1+0.5 \left( 31785 -31397.1 \right) = 31591.1m \] \[ \hat{\dot{x}}_{5,5}= 76.2+0.4 \left( \frac{31785 -31397.1}{5} \right) = 107.2m/s \] \[ \hat{\ddot{x}}_{5,5}=~ 1.8+0.1 \left( \frac{31785 -31397.1}{0.5 \times 5^{2}} \right) = 4.9m/s^{2} \] | \[ \hat{x}_{6,5}= 31591.1+5 \times 107.2 + 4.9 \times \frac{5^{2}}{2} = 32188.5m \] \[ \hat{\dot{x}}_{6,5}= 107.2 + 4.9 \times 5 = 131.7m/s \] \[ \hat{\ddot{x}}_{6,5}= 4.9m/s^{2} \] |

6 | \( 32215m \) | \[ \hat{x}_{6,6}=~ 32188.5+0.5 \left( 32215 -32188.5 \right) =32201.7m \] \[ \hat{\dot{x}}_{6,6}= 131.7+0.4 \left( \frac{32215 -32188.5}{5} \right) = 133.9m/s \] \[ \hat{\ddot{x}}_{6,6}=~ 4.9+0.1 \left( \frac{32215 -32188.5}{0.5 \times 5^{2}} \right) =5.1m/s^{2} \] | \[ \hat{x}_{7,6}= 32201.7+5 \times 133.9 + 5.1 \times \frac{5^{2}}{2}=32935.1m \] \[ \hat{\dot{x}}_{7,6}= 133.9 + 5.1 \times 5=159.5m/s \] \[ \hat{\ddot{x}}_{7,6}= 5.1m/s^{2} \] |

7 | \( 33130m \) | \[ \hat{x}_{7,7}=~ 32935.1+0.5 \left( 33130 -32935.1 \right) =33032.5m \] \[ \hat{\dot{x}}_{7,7}= 159.5+0.4 \left( \frac{33130 -32935.1}{5} \right) =175.1m/s \] \[ \hat{\ddot{x}}_{7,7}=~ 5.1+0.1 \left( \frac{33130 -32935.1}{0.5 \times 5^{2}} \right) =6.7m/s^{2} \] | \[ \hat{x}_{7,6}= 33032.5m+5 \times 175.1 + 6.7 \times \frac{5^{2}}{2}=33991.3m \] \[ \hat{\dot{x}}_{7,6}= 175.1 + 6.7 \times 5=208.5m/s \] \[ \hat{\ddot{x}}_{7,6}= 6.7m/s^{2} \] |

8 | \( 34510m \) | \[ \hat{x}_{8,8}= 33991.3+0.5 \left( 34510 -33991.3 \right) =34250.7m \] \[ \hat{\dot{x}}_{8,8}=208.5+0.4 \left( \frac{34510 -33991.3}{5} \right) =250m/s \] \[ \hat{\ddot{x}}_{8,8}=~ 6.7+0.1 \left( \frac{34510 -33991.3}{0.5 \times 5^{2}} \right) =10.8m/s^{2} \] | \[ \hat{x}_{9,8}= 34250.7+5 \times 250 + 10.8 \times \frac{5^{2}}{2}=35635.8m \] \[ \hat{\dot{x}}_{9,8}= 250 + 10.8 \times 5=304.1m/s \] \[ \hat{\ddot{x}}_{9,8}= 10.8m/s^{2} \] |

9 | \( 36010m \) | \[ \hat{x}_{9,9}=~ 35635.8+0.5 \left( 36010 -35635.8 \right) =35822.9m \] \[ \hat{\dot{x}}_{9,9}= 304.1+0.4 \left( \frac{36010 -35635.8}{5} \right) =334m/s \] \[ \hat{\ddot{x}}_{9,9}=~ 10.8+0.1 \left( \frac{36010 -35635.8}{0.5 \times 5^{2}} \right) =13.8m/s^{2} \] | \[ \hat{x}_{10,9}= 35822.9+5 \times 334+ 13.8 \times \frac{5^{2}}{2}=37665.8m \] \[ \hat{\dot{x}}_{10,9}= 334 + 13.8 \times 5=403.1m/s \] \[ \hat{\ddot{x}}_{10,9}= 13.8m/s^{2} \] |

10 | \( 37265m \) | \[ \hat{x}_{10,10}=~ 37665.8+0.5 \left( 37265 -37665.8 \right) =37465.4m \] \[ \hat{\dot{x}}_{10,10}= 403.1+0.4 \left( \frac{37265 -37665.8}{5} \right) =371.1m/s \] \[ \hat{\ddot{x}}_{10,10}=~ 13.8+0.1 \left( \frac{37265 -37665.8}{0.5 \times 5^{2}} \right) =10.6m/s^{2} \] | \[ \hat{x}_{11,10}= 37465.4+5 \times 371.1 +10.6 \times \frac{5^{2}}{2}=39453.5m \] \[ \hat{\dot{x}}_{11,10}= 371.1 + 10.6 \times 5=424.2m/s \] \[ \hat{\ddot{x}}_{11,10}= 10.6m/s^{2} \] |

The following charts compare the true value, measured values and estimates for the range, velocity and acceleration for the first 50 seconds.

As you can see, the \( \alpha -\beta -\gamma \) filter with dynamic model equations that include acceleration can track the target with constant acceleration and eliminate the lag error.

But what will happen in the case of a maneuvering target? The target can suddenly change the flight direction by making a turn. The true target dynamic model can also include a jerk (changing acceleration). In such cases, the \( \alpha -\beta -\gamma \) filter with constant \( \alpha -\beta -\gamma \) coefficients will produce the estimation errors and in some cases lose the target track.

The Kalman filter can handle the uncertainty in the dynamic model, and it is going to be our next topic right after the summary.

There are many types of \( \alpha -\beta -(\gamma) \) filters and they are based on the same principle:

- The current state estimation is based on the state update equations.
- The next state estimation (prediction) is based on the dynamic model equations.

The main difference between these filters is the selection of weighting coefficients \( \alpha -\beta -(\gamma) \). Some filter types use constant weighting coefficients, other compute weighting coefficients for every filter iteration (cycle).

The choice of the \( \alpha \), \( \beta \) and \( \gamma \) is crucial for proper functionality of your estimation algorithm. Another important issue is an initiation of the filter, i.e. providing the initial value for the first filter iteration.

The following list includes the most popular \( \alpha -\beta -(\gamma) \) filters:

- Wiener Filter
- Bayes Filter
- Fading-memory polynomial Filter
- Expanding-memory (or growing-memory) polynomial Filter
- Least-squares Filter
- Benedict–Bordner Filter
- Lumped Filter
- Discounted least-squares \( \alpha -\beta \) Filter
- Critically damped \( \alpha -\beta \) Filter
- Growing-memory Filter
- Kalman Filter
- Extended Kalman Filter
- Unscented Kalman Filter
- Extended Complex Kalman Filter
- Gauss-Hermite Kalman Filter
- Cubature Kalman Filter
- Particle Filter

I hope to write a tutorial about some of these filters in the future. But this tutorial is about the Kalman Filter and this is the topic of our next example.