Target audience: Advanced

Estimated reading time: 5'

In this article, we embark on an exploration of the Kalman optimal filter, employing the Scala programming language for its implementation. Renowned in the realms of signal processing and statistical analysis, the Kalman filter serves as a potent tool to measure or estimate noise arising from processes and the disturbances introduced by measurement instruments.

Table of contents

Overview

Covariance

Process and measurement noises

Notes:

The implementation relies on Scala 2.12.4
For the sake of readability of the implementation of algorithms, all non-essential code such as error checking, comments, exception, validation of class and method arguments, scoping qualifiers or import is omitted.

Overview

A Kalman filter serves as an ideal estimator, determining parameters from imprecise and indirect measurements. Its goal is to reduce the mean square error associated with the model's parameters. Being recursive in nature, this algorithm is suited for real-time signal processing. However, one notable constraint of the Kalman filter is its need for the process to be linear, represented as y = a.f(x) + b.g(x) + ....

Both the process and measurements introduce gaussian noise that affects the state.

Illustration of the Kalman filter as an estimator

Covariance

The Kalman filter represents the estimated state and error state as a covariance matrix. The covariance of a random vector x = { .. x .. } is a n by n positive definite, symmetric matrix with the variance of each variable as diagonal elements \[cov(\mathbf{x})= E[(\mathbf{x} - \overline{\mathbf{x}})(\mathbf{x} - \overline{\mathbf{x}} )^{t}] = \int_{-\infty }^{\infty} .. \int_{-\infty }^{\infty}(\mathbf{x} - \overline{\mathbf{x}})(\mathbf{x} - \overline{\mathbf{x}} )^{t}\,p(\mathbf{x})\,dx_{1} .. dx_{n}\] Such a matrix can be diagonalized by computing the eigenvectors (or basis vectors) in order to decouple the contribution of the noise or errors.

Process and measurement noises

The state of a deterministic time linear dynamic system is the smallest vector that summarizes the past of the system in full and allow a theoretical prediction of the future behavior, in the absence of noise. If x(k) is the n-dimension state vector at step k, u(k) the input vector, w(k) the unknown process noise vector normalized for a zero mean, and R(k) the covariance matrix for the measurement noise at step k, z the actual measurement of the state at step k, then \[\mathbf{x}_{k+1} = A_{k}\mathbf{x}_{k} + B_{k}\mathbf{u}_{k} + \mathbf{w}_{k}\,\,\,(1)\,\,with\,\,R_{k} = E[\mathbf{w}_{k}.\mathbf{w}_{k}^T]\\\mathbf{z}_{k} = H_{k}.\mathbf{x}_{k} + \mathbf{v}_{k}\,\,\,(2)\,\,\,\,with\,\,\,Q_{k} = E[\mathbf{v}_{k}.\mathbf{v}_{k}^T]\] where H(k) is the measurement equation related to the state A(k) and Q(k) is covariance matrix of the process noise at step k. We assume that the covariance matrix for the measurement noise, R and the covariance matrix for the error noise Q follow a Gaussian probability distribution.

We use the matrix classes and methods defined in the Apache Common Math open source library Commons Math 3.3

Filter

We leverage the support for functional constructs provided in Scala. Validation of method arguments, exceptions and non essential supporting methods are omitted for the sake of readability of the code snippets.

First we need to define the noise q generated by the linear process and the noise r generated by the measurement device. Those functions are defines as members of the QRNoise class. These white noise are indeed Gaussian random distributions for the noise on the process and measurement. The validation that the noise distribution follows a normal distribution is omitted.

case class QRNoise(qr: XY, white: Double=> Double) {
    // Compute the white noise for process Q
   private def q = white(qr._1)


    // Compute the white noise for measurement R
   private def r = white(qr._2)


    // Compute the white noise of the measurement  
   def noisyQ: Array[Double] = Array[Double](q, q)


    // Compute the white noise on the measurement
   def noisyR: Array[Double] = Array[Double](r, r)
}

Next, we need to define some internal types The discrete Kalman class, DKalman class has two objectives:

Encapsulates the types and method defined in Apache Common Math used in the generation of the state and error equations
Manage the state of the process

The class DKalman takes 5 arguments

A: State transition matrix (or model) defined in the first state equation (line 4)
B: Control/input matrix/model of the state equation (line 5)
H: Observation matrix/model of the prediction equation(line 6)
P: Noise covariance matrix (line 7)
qrNoise: Implicit process/model and measurement, observation noises

These model input parameters are used to compute the following values

Q Process white noise (line 12)
R Measurement white noise (line 17)
filter Instance of the Apache common math Kalman filter class (line 21)
x Current state vector (line 22)

type DblMatrix = Array[Array[Double]]

final protected class DKalman(
     A: DblMatrix,  
     B: DblMatrix,  
     H: DblMatrix, 
     P: DblMatrix)(implicit qrNoise: QRNoise) {
 
  type XYSeries = Array[(Double, Double)]

       // Process related white noise (mean = 0.0)
  private[this] val Q = new DblMatrix(A.size).map(_ =>
      Array.fill(A(0).size)(qrNoise.qr._1) 
  )

       // Measurement related white noise (mean = 0.0)
  private[this] val R = new DblMatrix(A.size).map(_ => 
      Array.fill(A(0).size)(qrNoise.qr._2) 
  )

  private var filter: KalmanFilter = _
  private var x: RealVector = _
  
   // Main filtering routine
  def filter(xt: XYSeries): XYSeries  = {}
  private def initialize(input: DblVector): Unit = {}
   
   // Compute the new state of the Kalman iterative computation
  private def newState: DblVector = {}

  // Compute the least squared errors of two vectors of type 'RealVector'
  def lsError(x: RealVector, z: RealVector): Double = {}
}

The method filter. below implements the basic sequences of the execution of an iteration of the update of the state of the process
1. predict the state of the process at the next step (x, A)
2. extract or generate noise for the process and the measurement devices (w, v)
3. update the state of the process (x)
4. computes the error on the measurement (z)
5. adjust the covariance matrices

Note: The control input u and initial state x0 are defined as arguments of the main method because they are independent from the model.

The two main stages of the Kalman filter are
- Initialization of the components used in the prediction and correction equation initialize (line 8)
- Execution of the prediction/correction cycle nextState (line 11)

import org.apache.commons.math3.linear._
import org.apache.commons.math3.filter._

def filter(xt: XYSeries): XYSeries = xt.map{ 
  case (x, y) => {

     // Initialize the filter
     initialize(Array[Double](x, y))  

     // Extract the new state a two values vector
     val nState = newState
     (nState(0), nState(1))
  }

State equation

The method newState implements the iterative state equation of the model
x = A.x + B.u + Q (line 7)
z - H.x + R (line 11)
described in the introduction.

The prediction phase is executed by invoking the Kalman.predict method of the Apache Commons Math library (line 6)and the correction phase relies on the Kalman.correct of the same Java library (line 14).

private def newState: Vector[Double] = {

    // Update the filter with the predictive value for x
    // and update it with the A transition matrix with the 
    // process noise qr.Q
    filter.predict
    x = A.operate(x).add(qrNoise.noisyQ) 

    // Compute the measured value z with the new update value 
    // using the measurement-statement dependency matrix H
    val z = H.operate(x).add(qrNoise.noisyR)
  
    // Update the filter with the new estimated measure z
    filter.correct(z)
    filter.getStateEstimation 
}

The method initalize create two instances of classes defined in the Apache commons math

pModel to model the process with the relevant matrices (line 2)
mModel for the measurement (line 3)

def initialize(input: Vector[Double]): Unit = {   
    val pModel = new DefaultProcessModel(A, B, Q, input, P)
    val mModel = new DefaultMeasurementModel(H, R)
   
    // Create a Kalman filter with a model pModel for 
    // the process and a model mModel for the measurement.
    filter = new KalmanFilter(pModel, mModel)

    // Conversion to Apache Commons Math internal types
    x = new ArrayRealVector(input)
}

Lastly, we need to compute the least squares error of the predicted states compared to the actual state values. The type RealVector is defined in the Apache commmon math library.

def lsError(x: RealVector, z: RealVector): Double = {
   val sumSqr = x.toArray

             .zip(z.toArray)

             .map(xz => (xz._1 - xz._2))
             .map( x => x*x).sum

   sqrt(sumSqr)
}

Use case

We will use a simple example of the Newton law of gravity. If x is the weight of an object, the differential equation can be integrated with a step 1 as follows \[\frac{\mathrm{d}^2 y_{t}}{\mathrm{d} t^{2}} + g = 0\,\,\Rightarrow\\ y_{t+dt} = y_{t}+ \dot{y}_{t}\,dt - \frac{g}{2}\,dt^{2}\,\,\,;\,\,\dot{y}_{t+1} = \dot{y}_{t} - g\,dt\,\,\,(\mathbf{3})\] The state vector x(k) for object at time k is defined by \[\mathbf{x}_{k} = [y_{k},\frac{\mathrm{d} y_{k}}{\mathrm{d} x}]^{T}\] and the state equation \[\mathbf{x}_{k+1} = A_{k}.\mathbf{x}_{k} + B_{k}.\mathbf{u}_{k}\,\,\,with\,\,\,A_{k} =\begin{vmatrix} 1 & dt\\ 0 & 1 \end{vmatrix}\,\,,\,B_{k}=\begin{vmatrix}0.5.dt.dt \\ dt \end{vmatrix}\] We use the Apache Commons Math library version 3.0 (Apache Commons Math User Guide to filter and predict the motion of a body subjected to gravity.

val dt = 0.05
val g = 9.81
val initialHeight = 100.0
val initialSpeed = 0.0
val variance = 0.014

  // Forces an implicit type conversion
val A: DblMatrix = (
   (1.0, dt),
   (0.0, 1.0)
)
val B: DblMatrix = (
    0.5*g*dt*dt, 
    g*dt
)
val H: DblMatrix = (1.0, 0.0)
val Q: DblMatrix = (
   (0.25*variance*pow(dt, 4), variance*pow(dt,3)/2), 
   (variance*pow(dt,3)/2,     variance*dt*dt)
)
val P0: DblMatrix = (
   (0.02, 0.0), 
   (0.0, 0.03)
)

  // Initialize the drop at 100 feet with no speed
val x0 = Array[Double](initialHeight, initialSpeed)
  
val qrNoise = new QRNoise((0.7, 0.3), (m: Double) => m*Random.nextGaussian)   

  // Create the process and noise models
val model = new DKalman(A, H, P0))
val output = model.filter(trajectory)

The state transition matrix A is initialized with the diagonal element (x and dx/dt) of 1 and dt (lines 8 - 10). The control vector B implements the coefficients of the first equation (3) with the values g*dt*dt/2 and g.dt (lines 12 -14). The trajectory is the only variable that is actually observed (speed and acceleration are not observed). Therefore, the measurement matrix H has only one non-zero entry (line 16).

The values for the noise on trajectory and speed for the model and the measurement is defined by the matrix Q (lines 17 - 19). The covariance matrix P0 (lines 21 - 23) completes the initialization for the process and measurement.

Thank you for reading this article. For more information ...

Sunday, May 30, 2021

Discrete Kalman Predictor in Scala

Overview

Covariance

Process and measurement noises

Filter

State equation

Use case

References

Contact Form

Equation Editor

Popular Posts