KalmanFtX

This control allows one to easily incorporate the power of linear Kalman filtering into their application. The Kalman Filter is similar to least squares fitting, but allows for real-time updates to the fit. The control allows user entries of the Process Noise (Q) matrix, the Dynamics (Phi) matrix, the Partials (H) matrix, the Measurement (Z) vector, the initial Covariance (P) matrix, and the initial State (X) vector.  A single call to KalmanFtX propagates the state vector and covariance matrix, adds the Q process noise to the covariance matrix, calculates the gain, updates covariance matrix, and then updates the state vector.  The MathLibX user's manual shows a clear and detailed example of a typical filter design.

A single call consists simply of "call KalmanFtX1.Kalman(Z, R, X, P, H, Q, Phi, Nx, Nz)" where Nx are the number of states and Nz are the number of measurements at this update.  Internal to the control, the update process performs the following functions:

A simple Kalman filter example: 

' This example filter accepts periodic position measurements and updates position, velocity, and acceleration states. 
' A detailed explaination of this example can be found in the MathLibX user's manual.

Private Sub Kalman_Filter()

    Static sigd(2000) As Double
    Static co(10) As Double
    Static std(10) As Double
    Static datalineX(2) As Double, datalineY(2) As Double

    Dim i As Long, j As Long
    Dim npt As Long
    Dim t As Double

    ' set up data
    npt = 1024  ' number of data points

    ' fill in some example data to filter
    For i = 0 To npt - 1
         dataX(i) = i
         dataY(i) = Sin(dataX(i) * 0.05) + 0.4 * Cos(dataX(i) * 0.15) + 1# - Rnd(1) _
             + 0.7 * Cos(dataX(i) * 0.4)
     Next i

    ' set P (covariance), Q (process noise), Phi (dynamics). 
    For i = 0 To 2
        For j = 0 To 2
            P(i, j) = 0#
            if i=j then
                Phi(i, j) = 1.0
            else
                 Phi(i, j) = 0#
             end if
            Q(i, j) = 0#
        Next j
    Next i

    ' init covariance
    P(0, 0) = 10 ^ 2
    P(1, 1) = 4 ^ 2
    P(2, 2) = 2 ^ 2

    ' Process noise
    Q(0, 0) = 0#
    Q(1, 1) = 0#
    Q(2, 2) = 0.00000001

    ' step (e.g., 1 sec sample period)
    t = 1#
    Phi(0, 1) = t
    Phi(0, 2) = 0.5 * t ^ 2
    Phi(1, 2) = t

    ' position measurements only (no velocity or accel measurements in this example)
    H(0, 0) = 1

    ' Initalize states with first measurement
    x(0) = dataY(0)
    x(1) = 0#
    x(2) = 0#

    ' Measurement Noise Variance
    R(0, 0) = 0.5 ^ 2

    ' filter  data
    For i = 1 To npt - 1
       Z(0) = dataY(i)
       call KalmanFtX1.Kalman(Z(0), R(0, 0), x(0), P(0, 0), H(0, 0), Q(0, 0), Phi(0, 0), 3, 1)
       debug.print  x(0), x(1), x(2)   ' filtered positions, velocity, and acceleration 
    Next i

End Sub