Friday, November 8, 2013

Discrete Kalman Optimal Estimator

Objective
This post is an introduction to the 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 deterministic discrete 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 omitted

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

Implementation
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: 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 DlbMatrix = 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))
}}

The method newStateimplements 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: DblVector = {

   // 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: DblVector): 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 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
Introduction to Kalman Filter University of North Carolina G. Welsh, G. Bishop 2006
Scala for Machine Learning - Patrick Nicolas - Packt Publishing

No comments:

Post a Comment