Limitations of the Linear Kalman Filter

 Target audience: Beginner

Estimated reading time: 5'

Posts history

Newsletter: Geometric Learning in Python 

Frustrated with the results from your Kalman Filter? The issue might be the non-linearity in the process you're modeling. 

This article explores the impact of non-linearity on the accuracy of state predictions made by the linear Kalman filter.

Table of contents

Linear Kalman Filter

      Theory

      Limitations

Implementation

      Setup

      Prediction

      Update

Evaluation

      Use case

      Study

References

Appendix

What you will learn: The impact of non-linearity of a dynamic process on the accuracy of the state predicted by a linear Kalman filter.

Notes: 

• Environments: Python 3.11,  Matplotlib 3.9, Numpy 2.1.2
• Source code is available at Github.com/patnicolas/Data_Exploration/Filter
• To enhance the readability of the algorithm implementations, we have omitted non-essential code elements like error checking, comments, exceptions, validation of class and method arguments, scoping qualifiers, and import statement.

Linear Kalman estimator

A deep dive in the underlying assumption and mathematics of Kalman Filter [ref 1] is beyond the scope of this article.

Theory

The linear Kalman filter, sometimes referred as standard Kalman filter is used for systems that can be described by linear state-space models and  assumes that the system’s dynamics and observation models are linear [ref 2].

Fig. 1 Visualization of the Linear Kalman filter algorithm

After initialization, the linear Kalman Filter forecasts the system's state for the upcoming step and estimates the uncertainty associated with this prediction.

Considering $A_n$ as the state transition model applied to the state $x_{n-1}$, $B_n$ as the control input model applied to the control vector $u_n$ if it exists, $Q_n$ as the covariance of the process noise, and $P_n$ as the error covariance matrix, the forecasted state $x$$x$is \[\begin{matrix} \widetilde{x}_{n/n-1}=A_{n}.\widetilde{x}_{n-1/n-1} + B_{n}.u_{n} \ \ (1)\\ P_{n/n-1}=A_{n}.P_{n-1/n-1}.A_{n}^{T}+Q_{n} \ \ (2) \end{matrix}\]

Upon receiving a measurement, the Kalman Filter adjusts or corrects the forecast and uncertainty of the current state. It then proceeds to predict future states, continuing this process. 

Thus, with a measurement $z_{n-1}$, a state $x_{n-1}$, and the innovation $S_n$, the Kalman Gain $G_{}$ and the error covariance $P_{}$ are calculated according. \[\begin{matrix} S_{n}=H.P_{n/n-1}.H^{T} +R_{n} \ \ \ \ \  (3)\ \ \ \ \ \ \ \ \ \ \ \ \ \\ G_{n} = \frac{1}{S_{n}}.P_{n/n-1}.H^{T}\ \ \ \ \  (4) \ \ \ \ \ \ \ \ \ \ \ \ \ \ \  \ \ \ \ \ \ \\ \widetilde{x}_{n/n} = \widetilde{x}_{n/n-1}+G_{n}(z_{n}-H.\widetilde{x}_{n/n-1}) \ \ \ (5) \\ g_{n}=I - G_{n}.H \ \ \ \ \ (6) \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \\ P_{n,n}= g_{n}.P_{n/n-1}.g_{n}^{T}+G_{n}.R_{n}.G_{n}^{T} \ \ \ \ (7) \ \ \end{matrix}\]

Fig. 2 Prediction - Update cycle for each measurement

Limitations

The linear Kalman filter is a powerful tool for estimating the state of a linear dynamic system in the presence of uncertainty. However, it has several limitations, particularly when applied to real-world systems that might not perfectly match its assumptions. Here are the key limitations:

• Assumption of Linearity: The Kalman Filter assumes that both the system dynamics and the measurement processes are linear. The relationship between the current state and the next state and the relationship between the state and the observations should be linear functions.
• Assumption of Gaussian Noise:  The Kalman Filter assumes that both process noise and measurement noise are Gaussian-distributed with known mean and covariance
• Strict Accuracy of Models: The Kalman Filter requires an accurate mathematical model of the system (i.e., the transition matrix, control matrix, observation matrix, and their respective noise covariances).
• Sensitivity to Initial Conditions: The Kalman Filter's performance is sensitive to the choice of initial conditions for the state estimate and error covariance matrix.
• Time-Invariant Covariance Matrices: The Kalman Filter assumes that the process and measurement noise covariance matrices remain constant over time (or are known in advance if they change)

Implementation

Setup  

The initial step involves programmatically implementing a linear Kalman filter using an object-oriented approach. To achieve this, we define a class named LinearKalmanFilter, which includes two constructors:

• __init__: This is the default constructor and can be fully specified for a standard linear Kalman filter.
• build: This alternative constructor is used when there is no control input, and both the process and measurement noise matrices have zero covariance.

The arguments of the constructors defines all the parameter of the Kalman filter:

_x0 : Initial values for the estimated state

_P0 : Initial values for the error covariance matrix

_A : State transition matrix (from state x[n-1] to state x[n])

_H : States to observations (or measurements) matrix

_Q : Process noise covariance matrix

_R : Observation or measurement matrix

_u0 : Optional initial value of the control variables

_B :  Optional control matrix (No control if None)

class LinearKalmanFilter(object):

     # Default constructor for fully defined filter
    def __init__(self,
                 _x0: np.array,
                 _P0: np.array,
                 _A: np.array,
                 _H: np.array,
                 _Q: np.array,
                 _R: np.array,
                 _u0: np.array = None,
                 _B: np.array = None) -> None:
        self.x = _x0
        self.P = _P0
        self.A = _A
        self.H = _H
        self.Q = _Q
        self.R = _R
        self.u = _u0
        self.B = _B


    #   Alternative constructor for the simplified Kalman filter:
    #  - No control input
    #   - No covariance for process and measure noise
    @classmethod
    def build(cls, _x0: np.array, _P0: np.array, _A: np.array, _H: np.array, qr: (float, float)) -> Self:
        dim = len(_x0)
        Q = np.eye(dim)*qr[0]
        R = np.eye(1)*qr[1]
        return cls(_x0, _P0, _A, _H, Q, R)

Prediction

The prediction method, predict, implements the equations (1) and (2) with the process noise w passed as argument.

Note: We are using the Numpy multiplication operator @ instead of numpy.dot method.

def predict(self, w: np.array) -> NoReturn:
     # State:  x[n] = A.x~[n-1] + B.u[n-1] + v
     self.x = self.A @ self.x + w if self.B is None 

                  else self.A @ self.x + self.B @ self.u + w          # Eq (1)

    # Error covariance:  P[n] = A[n].P[n-1].A[n]^T + Q[n]
    self.P = self.A @ self.P @ self.A.T + self.Q                    # Eq (2)

Update

The method update from class LinearKalmanFilter implements the computation of innovation and Kalman gain (equations 3 & 4) and update of state, x (equation 5)and error covariance matrix P (equation 7).

def update(self, z: np.array) -> NoReturn:
     # Innovation:  S[n] = H.P[n-1].H^T + R[n]
     S = self.H @ self.P @ self.H.T + R                 # Eq (3)

        
     # Gain: G[n] = P[n-1].H^T/S[n]
     G = self.P @ self.H.T @ np.linalg.inv(S)       # Eq (4)   

     # State estimate y[n] = z[n] - H.x
     y = z - self.H @ self.x

     self.x = self.x + G @ y              # Eq (5)

     # Update error covariance matrix
     g = np.eye(self.P.shape[0]) - G @ self.H        # Eq (6)

     self.P = g @ self.P               # Eq (7)

Evaluation

The objective is to evaluate and quantify the limitation of the Kalman filter for non-linear state update (i.e. x[n] = f(x[n-1]) with f non-linear).

Use case

Our user scenario is the ubiquitous tracking of an object of coordinates x and y on a 2D plan. The plotting {x, y} defines the trajectory in 2D.

\[ x_{k}=x_{k-1}+ \dot{x}_{k-1}.\Delta t+\ddot{x}_{k-1} \frac{(\Delta t^2)}{2} \]

\[ \dot{x}_{k}=\dot{x}_{k-1}+\ddot{x}_{k-1}.\Delta t \]

The derivative  vx = dx/dt (resp. vy = dy/dt) represents the velocity of the object along x (resp. y) axes. The second order derivative d^2x/dt^2 (resp. d^2y/dt^2) defines the acceleration along the x (resp. y) axes.

The force applied to the object is defined as F = m.acceleration, therefore the acceleration is the external control (variable u).

The state prediction can be implemented as the following matrix equation.

\[ x_{k}= \begin{bmatrix} x_{k} \\ y_{k} \\ \dot{x}_{k} \\ \dot{y}_{k} \end{bmatrix} \begin{bmatrix} 1 & 0 & \Delta t & 0  \\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{bmatrix} x_{k-1}+\begin{bmatrix} \frac{1}{2} (\Delta t)^2 &  0\\ 0 & \frac{1}{2} (\Delta t)^2  \\ \Delta t & 0 \\ 0 & \Delta t \\ \end{bmatrix} \begin{bmatrix} \ddot{x}_{k-1} \\ \ddot{y}_{k-1} \end{bmatrix} \]

The evaluation code initializes the components of the linear Kalman filter, where acceleration serves as the control variable u $\mathrm{<span\; style="font-family:\; helvetica;"><span\; style="color:\; \#666666;">along\; with\; the\; control\; matrix\; </span><span\; style="color:\; \#2b00fe;">B</span></span>}$. The simulation, which runs for 200 time steps, with time updates defined as  t[n+1] = t[n] + dt is carried out using the simulate$$ method of class LinearKalmanFilter and described in the appendix.

dt = 0.1
ac = 0.5*dt*dt

# Variables of the linear Kalman Filter
x0 = np.array([[0.0], [np.pi], [0.8], [0.2]])
P0 = np.eye(x0.shape[0])
A = np.array([[1.0, 0.0, dt, 0.0], [0.0, 1.0, 0.0, dt], [0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0]])
B = np.array([[ac, 0.0], [0.0, ac], [dt, 0.0], [0.0, dt]])
H = np.array([[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0]])
u = np.array([[1.0], [0.8]])
        
q = 0.6      # Variance for Q process noise matrix
r = 1.0       # Variance for R measurement noise matrix
Q = np.eye(4) * q
R = np.eye(1) * r
      
kf = LinearKalmanFilter(x0, P0, A, H, Q, R, u, B)

num_points = 200

estimation = kf.simulate(num_points,
                                       lambda i: obs_generator_lin(i),
                                       np.array([[0.4], [0.6], [0.1], [0.2]]))

Study

We write 4 functions to generate synthetic observations or measurements for tracking the object {x, y} as follows:

• f(x) = x
• f(x) = x*x
• f(x) = exp(-x/10)
• f(x) = sqrt(20,000*x)

def obs_generator_lin(i: int) -> np.array:
       return np.array([[i], [i]])
       
def obs_generator_sqr(i: int) -> np.array:
       return np.array([[i], [i*i]])

def obs_generator_exp(i: int) -> np.array:
       return np.array([[i], [math.exp(-i*0.1)]])

def obs_generator_sqrt(i: int) -> np.array:
       return np.array([[i], [math.sqrt(20_000.0*i)]])

The experiment generates 200 values for x-axis [0, 200] for linear and non-linear (sqr(x) = x*x) state update.

Fig. 3 Measured vs estimated trajectory for linear and non-linear (sqr) for state update

Next we plot the trajectory on the 3D plane {x, y} for the 4 generators of observations or measurements.

Fig. 4 Measured vs estimated  2D trajectory over 200 observations for 4 different generators

As expected, in the linear model [x, y = x], the estimated state aligns well with the generated observations (Plot #1). However, in the model [x, y = x^2], the estimated state diverges from the measurements over time (Plot #2), as the compound error grows exponentially. Conversely, for the model [x, y = sqrt{x}], the deviation between the estimated state and measurements decreases over time (Plot #4). In the highly non-linear case (Plot #3), the estimated state fails to keep up with the measurements altogether.

References

[1] Tutorial: Kalman Filter - MIT course

[2] Introduction to Kalman Filter University of North Carolina G. Welsh, G. Bishop

-------------
Patrick Nicolas has over 25 years of experience in software and data engineering, architecture design and end-to-end deployment and support with extensive knowledge in machine learning. 
He has been director of data engineering at Aideo Technologies since 2017 and he is the author of "Scala for Machine Learning", Packt Publishing ISBN 978-1-78712-238-3

Appendix

The simulation generates a specified number of measurements (`num_measurements`) using a function or lambda expression, `measure`, which is passed as an argument. At each time step, the process follows these steps:

1. Generate a measurement,
2. Predict or estimate the state,
3. Update the state and error covariance.

def simulate(self,
                 num_measurements: int,
                 measure: Callable[[int], np.array],
                 cov_means: np.array) -> List[np.array]:
   return [self.__estimate_next_state(i, measure, cov_means) for i in range(num_measurements)]

def __estimate_next_state(self, 

          obs_index: int, 

          measure: Callable[[int], np.array], 

          noise: np.array) -> np.array:
    observed = measure(obs_index)      # 1
    self.predict(noise)               # 2
    self.update(observed)        # 3
    return self.x