Implementing a Kalman filter for a differential equation system

General Usage
1

I am trying to implement a very simple Kalman Filter for learning purposes. I am following the example here (example 2, page 5). I will summarize it here with a MWE so no need to really go to the PDF.

Consider the following system of equations

\begin{align} x_1' &= x_2 \\ x_2' &= -0.01x_2 - x_1 + \sin(2t) \end{align}

I implemented this in Julia as (all code is MWE).

using DifferentialEquations
using Plots
using Kalman
using Statistics
using Random
using LinearAlgebra 
using GaussianDistributions
using GaussianDistributions: ⊕ # independent sum of Gaussian r.v.

## Lets solve the ODE first without using Kalman Filters
function theo_ode!(du,u,p,t)
    du[1] = u[2]
    du[2] = -0.01*u[2] - u[1] + sin(2*t)
end

u₀ = [0, 0]                       # initial state vector
tspan = (0.0,50.0)                # time interval

prob = ODEProblem(theo_ode!,u₀,tspan)
sol = solve(prob)

plot(sol,linewidth=2,xaxis="t",label=["x1" "x2"],layout=(2,1))

The solution to the diff equation looks like
image

Now I am trying to use this linear differential equation system with Kalman filter, using the Kalman.jl package. The system can be transformed to a Kalman formulation

\mathbf{x}^{\prime}(t)=A \mathbf{x}(t)+B\mathbf{u}(t)

where \mathbf{x} = [x_1, x_2]^T, and

\begin{aligned} A&=\left[\begin{array}{rr} 0 & 1 \\ -1 & -0.01 \end{array}\right] \\ B &=\left[\begin{array}{ll} 1 & 0 \\ 0 & 1 \end{array}\right] \\ u(t)&= \left[\begin{array}{r} 0 \\ \sin (2 t) \end{array}\right] \end{aligned}

Modifying the example from Kalman.jl, I have the following code:

function run_kalman()
## converting the above differential equaiton into matrix form for kalman formulation
A = [0 1; -1 -0.01]
B = [1 0; 0 1]
Q = [0.0005 0.0; 0.0 0.0005]

# prior for time 0
x0 = [0., 0.]
P0 = [0.5 0.0; 0.0 0.5]

# measurement
H = [0.0 1.0]
R = [0.0005]

# (mock) data
ys = [[0.8],[1.4],[0.7],[-1.1],[-1.5],[-0.9],[-0.2],[1.3],[1.8],[-0.3],[-1.6],[-1.1],[-0.4],[1.1],[1.8],[0.6],[-0.3],[-0.7],[-0.8],[-0.1],[0.9],[0.3],[-0.7],[-0.3],[-0.4],[0.1],[0.7],[-0.4],[-0.9],[-0.2],[0.4],[-0.2],[0.1],[0.2],[-0.4],[0],[1],[0.5],[0.3],[0.6],[-0.4],[-0.9],[0],[0.4],[0.3],[0.8],[0.1],[-1.6],[-1.3],[-0.1]]

# filter (assuming first observation at time 1)
Δt = 0.01
N = collect(0:Δt:49.99) # set up the time from 0 to 50 increments of 0.01
 # the kalman update loop

p = Gaussian(x0, P0) # define the initial condition
ps = []
for t in N ## go through the times
   # use old value of p to update and get new value of p
   p = Gaussian(A*p.μ + B*[0, sin(2*t)], A*p.Σ*A' + Q)    

   # ignore the uddate step
   p, yres, _ = Kalman.correct(Kalman.JosephForm(), p, (Gaussian(ys[i], R), H))
   push!(ps, p) # save filtered density
end
return ps
end

ps = run_kalman();

Note that in the Kalman filter, I am ignoring the prediction step!!! I was to be able to replicate the original differential equation. I am under the impression that the update step of the algorithm should produce similar results to the differential equation (see figure 2 of the linked pdf), but the results are so crappy! Here is the plot of my \mathbf{x} state variable:

image

Why dosn’t it look like Figure 2 of the linked pdf? Am I using the package wrong?