State Extrapolation Equation

Before we start, I would like to introduce notation used through this tutorial:

  • Bold-face, lower-case letters refer to vectors, such as \( \boldsymbol{x} \)
  • Bold-face, capital letters refer to matrices, such as \( \boldsymbol{A} \)
  • Normal-face, lower-case letters refer to scalars or vector elements
  • Normal-face, capital letters refer to matrix elements

This first Kalman Filter equation that I would like to describe is the state extrapolation equation.

Using the state extrapolation equation, we can predict the next system state, based on the knowledge of the current state. It extrapolates state vector from the present (time step \( n \) ) to the future (time step \( n + 1 \) ).

The state extrapolation equation describes the model of the dynamic system. In the literature it is also called:

  • Predictor Equation
  • Transition Equation
  • Prediction Equation
  • Dynamic Model
  • State Space Model

The general form of the state extrapolation equation in a matrix notation is:

\[ \boldsymbol{\hat{x}_{n+1,n}=F\hat{x}_{n,n}+G\hat{u}_{n,n}+w_{n}} \]
Where:
\( \boldsymbol{\hat{x}_{n+1,n}} \) is a predicted system state vector at time step \( n + 1 \)
\( \boldsymbol{\hat{x}_{n,n}} \) is an estimated system state vector at time step \( n \)
\( \boldsymbol{\hat{u}_{n,n}} \) is a control variable or input variable - a measurable (deterministic) input to the system
\( \boldsymbol{w_{n}} \) is a process noise or disturbance - an unmeasurable input that affects the state
\( \boldsymbol{F} \) is a state transition matrix
\( \boldsymbol{G} \) is a control matrix or input transition matrix (mapping control to state variables)
Note: In the literature, the state transition matrix \( F \) is sometimes denoted by a Greek letter \( \Phi \).

The following figure provides a schematic description of the state extrapolation equation.

Kalman Filter Extrapolation

The state variables may represent attributes of the system that we wish to know something about.

For example, the moving airplane has three attributes: position, velocity, and acceleration.

You might ask yourself, which attributes are the state variables, and which attributes are the input to the system?

  • The moving mechanical systems have attributes such as: position, velocity, acceleration, drag.
  • A force that acts on a system should be considered an external forcing function, i.e., an input to the system that controls the state vector (position and velocity in the constant acceleration case).
  • Newton's second law tells us that \( F = ma \), thus we can consider acceleration as an external input to the system.
  • The position and the velocity are the primary state variables of interest.

For instance, in a spring system, the force applied to the spring \( F(t) \) is an input \( u(t) \), while the spring displacement \( x(t) \) is the system state.

Spring System

For the falling object, the inputs are the gravitational force \( F_{g} \) and the drag force \( F_{drag}(t) \), while the object height \( h(t) \) and velocity \( v(t) \) are the system states.

Spring System
Note: The process noise \( w_{n} \) do not typically appear directly in the equations of interest. Instead, this term is used to model the uncertainty in Covariance Extrapolation Equation.

Let's take a look on several examples of the state extrapolation equation.

Example - airplane constant acceleration model

Consider an airplane moving in three-dimensional space with constant acceleration. The state vector \( \boldsymbol{\hat{x}_{n}} \) that describes the estimated airplane position and velocity in cartesian coordinate system \( \left( x,y,z \right) \) is:

\[ \boldsymbol{\hat{x}_{n}}= \left[ \begin{matrix} \hat{x}_{n}\\ \hat{y}_{n}\\ \hat{z}_{n}\\ \hat{\dot{x}}_{n}\\ \hat{\dot{y}}_{n}\\ \hat{\dot{z}}_{n}\\ \end{matrix} \right] \]

Note: Don't confuse the estimated state vector \( \boldsymbol{\hat{x}_{n}} \) (bold-face font) and the estimated airplane position at x axis denoted by \( \hat{x}_{n} \) (normal-face font). They are both denoted by same letter in the literature, and I am trying to be consistent.

The control vector \( \boldsymbol{\hat{u}_{n}} \) that describes the airplane acceleration in cartesian coordinate system \( \left( x,y,z \right) \) is:

\[ \boldsymbol{\hat{u}_{n}}= \left[ \begin{matrix} \hat{\ddot{x}}_{n}\\ \hat{\ddot{y}}_{n}\\ \hat{\ddot{z}}_{n}\\ \end{matrix} \right] \]

The state transition matrix \( \boldsymbol{F} \) is:

\[ \boldsymbol{F}= \left[ \begin{matrix} 1 & 0 & 0 & \Delta t & 0 & 0\\ 0 & 1 & 0 & 0 & \Delta t & 0\\ 0 & 0 & 1 & 0 & 0 & \Delta t\\ 0 & 0 & 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 0 & 0 & 1\\ \end{matrix} \right] \]

The control matrix \( \boldsymbol{G} \) is:

\[ \boldsymbol{G}= \left[ \begin{matrix} 0.5\Delta t^{2} & 0 & 0 \\ 0 & 0.5\Delta t^{2} & 0 \\ 0 & 0 & 0.5\Delta t^{2} \\ \Delta t & 0 & 0 \\ 0 & \Delta t & 0 \\ 0 & 0 & \Delta t \\ \end{matrix} \right] \]

The state extrapolation equation is:

\[ \boldsymbol{\hat{x}_{n+1,n}=F\hat{x}_{n,n}+G\hat{u}_{n,n}} \]

\[ \left[ \begin{matrix} \hat{x}_{n+1,n}\\ \hat{y}_{n+1,n}\\ \hat{z}_{n+1,n}\\ \hat{\dot{x}}_{n+1,n}\\ \hat{\dot{y}}_{n+1,n}\\ \hat{\dot{z}}_{n+1,n}\\ \end{matrix} \right] = \left[ \begin{matrix} 1 & 0 & 0 & \Delta t & 0 & 0\\ 0 & 1 & 0 & 0 & \Delta t & 0\\ 0 & 0 & 1 & 0 & 0 & \Delta t\\ 0 & 0 & 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 0 & 0 & 1\\ \end{matrix} \right] \left[ \begin{matrix} \hat{x}_{n,n}\\ \hat{y}_{n,n}\\ \hat{z}_{n,n}\\ \hat{\dot{x}}_{n,n}\\ \hat{\dot{y}}_{n,n}\\ \hat{\dot{z}}_{n,n}\\ \end{matrix} \right] + \left[ \begin{matrix} 0.5\Delta t^{2} & 0 & 0 \\ 0 & 0.5\Delta t^{2} & 0 \\ 0 & 0 & 0.5\Delta t^{2} \\ \Delta t & 0 & 0 \\ 0 & \Delta t & 0 \\ 0 & 0 & \Delta t \\ \end{matrix} \right] \left[ \begin{matrix} \hat{\ddot{x}}_{n,n}\\ \hat{\ddot{y}}_{n,n}\\ \hat{\ddot{z}}_{n,n}\\ \end{matrix} \right] \]

The matrix multiplication results:

\[ \begin{cases} \hat{x}_{n+1,n} = \hat{x}_{n,n} + \hat{\dot{x}}_{n,n} \Delta t+ \frac{1}{2}\hat{\ddot{x}}_{n,n} \Delta t^{2}\\ \hat{y}_{n+1,n} = \hat{y}_{n,n} + \hat{\dot{y}}_{n,n} \Delta t+ \frac{1}{2}\hat{\ddot{y}}_{n,n} \Delta t^{2}\\ \hat{z}_{n+1,n} = \hat{z}_{n,n} + \hat{\dot{z}}_{n,n} \Delta t+ \frac{1}{2}\hat{\ddot{z}}_{n,n} \Delta t^{2}\\ \hat{\dot{x}}_{n+1,n} = \hat{\dot{x}}_{n,n} + \hat{\ddot{x}}_{n,n} \Delta t\\ \hat{\dot{y}}_{n+1,n} = \hat{\dot{y}}_{n,n} + \hat{\ddot{y}}_{n,n} \Delta t\\ \hat{\dot{z}}_{n+1,n} = \hat{\dot{z}}_{n,n} + \hat{\ddot{z}}_{n,n} \Delta t\\ \end{cases} \]

Let's see more examples.

Example - airplane constant velocity model

Consider an airplane moving in three-dimensional space with constant velocity. At the constant velocity the acceleration equals to zero, therefore the input variable \( \boldsymbol{\hat{u}_{n}} \) that describes the airplane acceleration in cartesian coordinate system \( \left( x,y,z \right) \) equals to zero:

\[ \boldsymbol{\hat{u}_{n}}= \left[ \begin{matrix} \hat{\ddot{x}}_{n}\\ \hat{\ddot{y}}_{n}\\ \hat{\ddot{z}}_{n}\\ \end{matrix} \right] = \left[ \begin{matrix} 0\\ 0\\ 0\\ \end{matrix} \right] \]

Thus, we can eliminate the control variable part of the state extrapolation equation: \( \boldsymbol{Gu_{n,n}=0} \)

The state extrapolation equation would be:

\[ \boldsymbol{\hat{x}_{n+1,n}=F\hat{x}_{n,n}} \]

\[ \left[ \begin{matrix} \hat{x}_{n+1,n}\\ \hat{y}_{n+1,n}\\ \hat{z}_{n+1,n}\\ \hat{\dot{x}}_{n+1,n}\\ \hat{\dot{y}}_{n+1,n}\\ \hat{\dot{z}}_{n+1,n}\\ \end{matrix} \right] = \left[ \begin{matrix} 1 & 0 & 0 & \Delta t & 0 & 0\\ 0 & 1 & 0 & 0 & \Delta t & 0\\ 0 & 0 & 1 & 0 & 0 & \Delta t\\ 0 & 0 & 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 0 & 0 & 1\\ \end{matrix} \right] \left[ \begin{matrix} \hat{x}_{n,n}\\ \hat{y}_{n,n}\\ \hat{z}_{n,n}\\ \hat{\dot{x}}_{n,n}\\ \hat{\dot{y}}_{n,n}\\ \hat{\dot{z}}_{n,n}\\ \end{matrix} \right] \]

The matrix multiplication results:

\[ \begin{cases} \hat{x}_{n+1,n} = \hat{x}_{n,n} + \hat{\dot{x}}_{n,n} \Delta t\\ \hat{y}_{n+1,n} = \hat{y}_{n,n} + \hat{\dot{y}}_{n,n} \Delta t\\ \hat{z}_{n+1,n} = \hat{z}_{n,n} + \hat{\dot{z}}_{n,n} \Delta t\\ \hat{\dot{x}}_{n+1,n} = \hat{\dot{x}}_{n,n}\\ \hat{\dot{y}}_{n+1,n} = \hat{\dot{y}}_{n,n}\\ \hat{\dot{z}}_{n+1,n} = \hat{\dot{z}}_{n,n}\\ \end{cases} \]

Example – falling object

Consider the free-falling object. The state vector includes the altitude \( h \) and the object's velocity \( \dot{h} \):

\[ \boldsymbol{\hat{x}_{n}}= \left[ \begin{matrix} \hat{h}_{n}\\ \hat{\dot{h}}_{n}\\ \end{matrix} \right] \]

The state transition matrix \( \boldsymbol{F} \) is:

\[ \boldsymbol{F}= \left[ \begin{matrix} 1 & \Delta t \\ 0 & 1 \\ \end{matrix} \right] \]

The control matrix \( \boldsymbol{G} \) is:

\[ \boldsymbol{G}= \left[ \begin{matrix} 0.5\Delta t^{2} \\ \Delta t \\ \end{matrix} \right] \]

The input variable \( \boldsymbol{\hat{u}_n} \) is:

\[ \boldsymbol{\hat{u}_n}= \left[ \begin{matrix} g \end{matrix} \right] \]

where \( g \) is the gravitational acceleration.

The state extrapolation equation looks like:

\[ \left[ \begin{matrix} \hat{h}_{n+1,n}\\ \hat{\dot{h}}_{n+1,n}\\ \end{matrix} \right] = \left[ \begin{matrix} 1 & \Delta t \\ 0 & 1 \\ \end{matrix} \right] \left[ \begin{matrix} \hat{h}_{n,n}\\ \hat{\dot{h}}_{n,n}\\ \end{matrix} \right] + \left[ \begin{matrix} 0.5\Delta t^{2} \\ \Delta t \\ \end{matrix} \right] \left[ \begin{matrix} g \end{matrix} \right] \]

The matrix multiplication results:

\[ \begin{cases} \hat{h}_{n+1,n} = \hat{h}_{n,n} + \hat{\dot{h}}_{n,n} \Delta t + 0.5 \Delta t^{2} g\\ \hat{\dot{h}}_{n+1,n} = \hat{\dot{h}}_{n,n} + \Delta t g\\ \end{cases} \]

State extrapolation equation dimensions

The following table specifies the matrix dimensions of the state extrapolation equation variables:

Variable Description Dimension
\( \boldsymbol{x} \) state vector \( n_{x} \times 1 \)
\( \boldsymbol{F} \) state transition matrix \( n_{x} \times n_{x} \)
\( \boldsymbol{u} \) input variable \( n_{u} \times 1 \)
\( \boldsymbol{G} \) control matrix \( n_{x} \times n_{u} \)
\( \boldsymbol{w} \) process noise vector \( n_{x} \times 1 \)

Liner time-invariant systems

This section of the tutorial deals with LTI (Linear Time-Invariant) systems. The Kalman Filter for systems with time-varying dynamics and the Kalman Filter for non-linear systems (Extended Kalman Filter) will be described later.

So, what is linear and what is time-invariant?

Linear systems are systems of equations in which the variables are never multiplied with each other but only with constants and then summed up. Linear systems are used to describe both static and dynamic relations between variables.

The linear system is a system whose output function \( \mathcal{F} \) satisfies the following equation:

\[ \mathcal{F} \left( a \times g \left( t \right) +b \times h \left( t \right) \right) = a \times \mathcal{F} \left( g \left( t \right) \right) + b \times \mathcal{F} \left( h \left( t \right) \right) \]

where:

\( a \) and \( b \) are constant real numbers

\( g \) and \( h \) are any arbitrary functions of an independent variable \( t \)

The linear system follows two basic rules:

  1. You can "factor out" constant multiplicative scale factors (the \( a \) and \( b \) above).
  2. The response of the system to a sum of inputs is the sum of the responses to each individual input separately.

A time-invariant system has a system function that is not a direct function of time.

Let's take an amplifier with gain (G = 10) as an example.

Amplifier

This system is time-invariant. Although the output of the system changes with a time, the system function is not time dependent.

A time-invariant system is one where a time delay (or shift) in the input sequence causes an equivalent time delay in the system's output sequence.

Previous Next