Target audience: Advanced

Estimated reading time: 35'

*Kalman optimal*filter using the Scala programming language as implementation. The Kalman filter is widely used in signal processing and statistical analysis to quantify or estimate noise created by a process and noise generated by measurement devices.

Overview

**A**

*Kalman filter*is an

*optimal estimator*that derives parameters from indirect and inaccurate observations. The objective is the algorithm is to minimize the

*mean square error*of the model parameters. The algorithm is recursive and therefore can be used for real-time signal analysis. The Kalman filter has one main limitation: it requires the process to be linear y = a.f(x) + b.g(x) + .... The state is impacted by

*Gaussian noise*in the process and measurement.

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.State Equation Model

The state of a

Apache Commons Math

*deterministic**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 stepk, 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.**Note**: 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 omittedApache Commons Math

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

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**: The state transition matrix (or model) defined in the first state equation (line 4)**B**: The control/input matrix/model of the state equation (line 5)**H**: The observation matrix/model of the prediction equation (line 6)**P**: The noise covariance matrix (line 7)**qrNoise**: The 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)

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 | 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)

1 2 3 4 5 6 7 8 9 10 11 12 13 | 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

described in the introduction.

The prediction phase is executed by invoking the

*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).1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 | 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)

1 2 3 4 5 6 7 8 9 10 11 | 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

Math.sqrt(sumSqr) }

Example

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

References

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.1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 | 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*Math.pow(dt, 4), variance*Math.pow(dt,3)/2), (variance*Math.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 a12=x/(dx/dt) = 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.References