Unscented Kalman Filter (additive noise) Example

Introduction

This notebook is designed to demonstrate how to use the StateSpace.jl package to execute the Unscented Kalman filter with additive noise for a non-linear State Space model. The example that has been used here closely follows the example given in a paper by Rambabu Kandepu, Bjarne Foss and Lars Imsland "Applying the unscented Kalman filter for nonlinear state estimation". The example is the Van der Pol oscillator (Section 4.1 on page 6 of the paper). You can read the paper for full details.

For those of you that do not need/want the explanation of the model and the code, you can skip right to the end of this notebook where the entire section of code required to run this example is given.

The Problem

The problem that we will consider here is that of predicting the position $\mathbf{x} = (x_1, x_2)$ of a Van der Pol oscillator given some noisy measurements of its position.

The Process Model

The dynamic equations describing the rate of change of position of the oscillator are:

$$ \begin{align} \frac{\mathrm{d}x_1}{\mathrm{d}t} &= -x_2, \\ \frac{\mathrm{d}x_2}{\mathrm{d}t} &= -\mu \left(1 - x_1^2 \right)x_2 + x_1, \\ \end{align} $$

where $\mu$ is a scalar parameter that defines the strength of the damping.
These equations can be written in a discrete manner required for solution with the unscented Kalman filter as follows:

$$ \begin{align} x_1^n &= x_1^{n-1} - \Delta t \times x_2^{n-1}, \\ x_2^n &= x_2^{n-1} - \Delta t \times \mu \left(1 - (x_1^{n-1})^2 \right)x_2^{n-1} + x_1^{n-1}, \\ \end{align} $$

where $\mathbf{x}^n = \left( x_1^n, x_2^n \right)$ represents the position of the oscillator at time step $n$ and $\Delta t$ is the time step between consecutive measurements.

The Observation Model

We assume that we measure the position of the oscillator directly and hence the observation function is simply $$ y^n(\mathbf{x}^n) = \left( x_1^n, x_2^n \right) $$

Setting up the problem

First we'll import the required modules



In [1]:

    
using StateSpace
using Distributions
using Gadfly
using DataFrames
using Colors

Define Kalman Filter Parameters

We now need to define the process and observation model according to the model described above.



In [12]:

    
Δt = 0.1 # Set the time step

#Set the process function
function processFunction(x::Vector, Δt::Float64=0.1, μ::Float64=0.3)
    x1 = zeros(x)
    x1[1] = x[1] + Δt * -x[2]
    x1[2] = x[2] + Δt * (-μ * (1 - x[1]^2) * x[2] + x[1])
    x1
end

#Set the observation function.
observationFunction(x::Vector) = x

#Set process noise covariance matrix
processCovariance = 1e-1*[0.1 0.0; 0.0 1e-3]

#Set the observation noise covariance
observationCovariance = 3e-2*eye(2)

#Create additive noise UKF model
ukfStateModel = AdditiveNonLinUKFSSM(processFunction, processCovariance, observationFunction, observationCovariance)









    Out[12]:





StateSpace.AdditiveNonLinUKFSSM{Float64}(processFunction,2x2 Array{Float64,2}:
 0.01  0.0   
 0.0   0.0001,observationFunction,2x2 Array{Float64,2}:
 0.03  0.0 
 0.0   0.03)

Generate noisy observations

Here we'll generate the noisy observations that will be passed to the Unscented Kalman Filter.

First we define the true initial position of the system



In [13]:

    
trueInitialState = [1.4, 0.0] #Set the true initial state of the oscillator









    Out[13]:





2-element Array{Float64,1}:
 1.4
 0.0

Now we can define the number of observations and generate the true and noisy observations



In [14]:

    
#Set the number of observations and generate the true values and the noisy
#observations.
numObs = 200
trueState = zeros(2,numObs)
noisyObs = zeros(2,numObs)
trueState[:,1] = trueInitialState
noisyObs[:,1] = trueInitialState + sqrt(observationCovariance)*randn(2)
for i in 2:numObs
    trueState[:,i] = processFunction(trueState[:,i-1])
    noisyObs[:,i] = trueState[:,i] + sqrt(observationCovariance)*randn(2)
end

Guess of initial state

Now we define the guess of the initial state of the system



In [15]:

    
initial_guess = MvNormal([0.0,2.0], 1.0*eye(2))









    Out[15]:





FullNormal(
dim: 2
μ: [0.0,2.0]
Σ: 2x2 Array{Float64,2}:
 1.0  0.0
 0.0  1.0
)

Perform Unscented Kalman Filter (additive noise) Algorithm

Now we have all of the parameters:

noisy observations
process (transition) and observation (emission) model paramaters
initial guess of state

We can use the Unscented Kalman Filter to predict the true underlying state (position of the oscillator).



In [16]:

    
filtered_state = filter(ukfStateModel, noisyObs, initial_guess)









    



SmoothedState{Float64}





    Out[16]:












    



201 estimates of 2-D process from 2-D observations
Log-likelihood: 701.8763985625116

NOTE: The log likelihood hasn't been implemented for the Unscented Kalman Filter and so 0.0 is just the default value set. It doesn't mean anything yet. We will sort this out soon.

Plot the results

Now we will plot the results of the $x_1$ position of the oscillator.

First we extract the mean $x_1$ positions and their variances from the filtered estimates.



In [17]:

    
x_data = 1:numObs
x1_array = Vector{Float64}(numObs)
x1Var_array = Vector{Float64}(numObs)
x1_Guess = initial_guess.μ[1]
x1Var_Guess = 2*sqrt(initial_guess.Σ.mat[1,1])
for i in x_data
    current_state = filtered_state.state[i]
    x1_array[i] = current_state.μ[1]
    x1Var_array[i] = 2*sqrt(current_state.Σ.mat[1,1])
end

Next we will create a dataframe. This is simply so the syntax is simple for plotting the ribbon digram which will represent the state along with the 95% confidence interval



In [18]:

    
df_fs = DataFrame(
    x = x_data*Δt,
    y = x1_array,
    ymin = x1_array - x1Var_array,
    ymax = x1_array + x1Var_array,
    f = "Filtered values"
    )









    Out[18]:




x y ymin ymax f
1 0.1 0.0 -2.0 2.0 Filtered values
2 0.2 1.4082183222429558 1.0125689430929183 1.8038677013929934 Filtered values
3 0.30000000000000004 1.3270343378454568 0.9983130422356324 1.6557556334552812 Filtered values
4 0.4 1.3669012029391918 1.055790931877306 1.6780114740010776 Filtered values
5 0.5 1.2077546471196439 0.9022328133118163 1.5132764809274715 Filtered values
6 0.6 1.2217532103709878 0.9181560197247857 1.52535040101719 Filtered values
7 0.7000000000000001 1.031687856399231 0.7288253108774647 1.3345504019209975 Filtered values
8 0.8 1.0947832148967058 0.7921859500532789 1.3973804797401328 Filtered values
9 0.9 0.9258291881280865 0.6234388909948582 1.2282194852613149 Filtered values
10 1.0 1.0220403440150887 0.7197155623609277 1.3243651256692497 Filtered values
11 1.1 0.9238642526737051 0.6217297870966805 1.2259987182507297 Filtered values
12 1.2000000000000002 0.8123584429913062 0.5102885411473409 1.1144283448352716 Filtered values
13 1.3000000000000003 0.731448825407698 0.4293676591073239 1.033529991708072 Filtered values
14 1.4000000000000001 0.5485454062659543 0.24643641739416416 0.8506543951377443 Filtered values
15 1.5000000000000002 0.18075414491182812 -0.12151970464536577 0.483027994469022 Filtered values
16 1.6 0.26437599860254213 -0.03824756405315499 0.5669995612582392 Filtered values
17 1.7000000000000002 0.14441207634414913 -0.1582566527119613 0.4470808054002595 Filtered values
18 1.8000000000000003 -0.14837234530998245 -0.4511410116514407 0.15439632103147582 Filtered values
19 1.9000000000000001 -0.19242752586392095 -0.4954061193476691 0.11055106761982716 Filtered values
20 2.0 -0.24205808341285984 -0.5451232952211964 0.06100712839547673 Filtered values
21 2.1 -0.3822325989378521 -0.6853454681064522 -0.07911972976925197 Filtered values
22 2.2 -0.5881898289882574 -0.8913700270104367 -0.28500963096607823 Filtered values
23 2.3000000000000003 -0.5970455348178784 -0.900302233730638 -0.29378883590511873 Filtered values
24 2.4000000000000004 -0.7197779164279073 -1.023052185760254 -0.4165036470955608 Filtered values
25 2.5000000000000004 -0.7468184649139971 -1.0501156387546082 -0.4435212910733858 Filtered values
26 2.6 -0.8809464568843236 -1.1842437827708043 -0.5776491309978429 Filtered values
27 2.7 -0.9373369430642993 -1.2406418851680265 -0.6340320009605719 Filtered values
28 2.8000000000000003 -1.0442395949223962 -1.34753730399854 -0.7409418858462522 Filtered values
29 2.9000000000000004 -1.178699589832252 -1.481985554421409 -0.8754136252430953 Filtered values
30 3.0000000000000004 -1.1774740704604292 -1.4807458036131589 -0.8742023373076994 Filtered values
&vellip &vellip &vellip &vellip &vellip &vellip

Next we create 3 distinguishable colors for the plot



In [19]:

    
n = 3
getColors = distinguishable_colors(n, Color[LCHab(70, 60, 240)],
                                   transform=c -> deuteranopic(c, 0.5),
                                   lchoices=Float64[65, 70, 75, 80],
                                   cchoices=Float64[0, 50, 60, 70],
                                   hchoices=linspace(0, 330, 24))









    Out[19]:

Finally we display the plot



In [25]:

    
oscillatorx1_state_plot = plot(
    layer(x=x_data*Δt, y=noisyObs[1,:], Geom.point, Theme(default_color=getColors[2])),
    layer(x=x_data*Δt, y=trueState[1,:], Geom.line, Theme(default_color=getColors[3])),
    layer(df_fs, x=:x, y=:y, ymin=:ymin, ymax=:ymax, Geom.line, Geom.ribbon),
    Guide.xlabel("Time (seconds)"), Guide.ylabel("x1"),
    Guide.manual_color_key("Colour Key",["Filtered Estimate", "Measurements","True Value "],[getColors[1],getColors[2],getColors[3]]),
    Guide.title("Unscented Kalman Filter (Additive) Example")
    )
display(oscillatorx1_state_plot)

Unscented Rauch-Tung-Striebel Smoother

The Unscented RTS smoother has been implemented to perform smoothing on the filtered estimates to get a better estimate of the true underlying state (as they say, hindsight is 20/20). This can be performed in an identical manner to the Linear case like so:



In [21]:

    
smoothed_state = smooth(ukfStateModel, filtered_state)









    



SmoothedState{Float64}





    Out[21]:












    



200 estimates of 2-D process from 2-D observations
Log-likelihood: 0.0

Now we can the results in the same way as above to visualize the smoothed result.



In [23]:

    
x_data = 1:numObs
x1_array = Vector{Float64}(numObs)
x1Var_array = Vector{Float64}(numObs)
x1_Guess = initial_guess.μ[1]
x1Var_Guess = 2*sqrt(initial_guess.Σ.mat[1,1])
for i in x_data
    current_state = smoothed_state.state[i]
    x1_array[i] = current_state.μ[1]
    x1Var_array[i] = 2*sqrt(current_state.Σ.mat[1,1])
end
df_fs = DataFrame(
    x = x_data*Δt,
    y = x1_array,
    ymin = x1_array - x1Var_array,
    ymax = x1_array + x1Var_array,
    f = "Smoothed values"
    )

n = 3
getColors = distinguishable_colors(n, Color[LCHab(70, 60, 240)],
                                   transform=c -> deuteranopic(c, 0.5),
                                   lchoices=Float64[65, 70, 75, 80],
                                   cchoices=Float64[0, 50, 60, 70],
                                   hchoices=linspace(0, 330, 24))
oscillatorx1_state_plot_smooted = plot(
    layer(x=x_data*Δt, y=noisyObs[1,:], Geom.point, Theme(default_color=getColors[2])),
    layer(x=x_data*Δt, y=trueState[1,:], Geom.line, Theme(default_color=getColors[3])),
    layer(df_fs, x=:x, y=:y, ymin=:ymin, ymax=:ymax, Geom.line, Geom.ribbon),
    Guide.xlabel("Time (seconds)"), Guide.ylabel("x1"),
    Guide.manual_color_key("Colour Key",["Filtered Estimate", "Measurements","True Value "],[getColors[1],getColors[2],getColors[3]]),
    Guide.title("Unscented Kalman Smoother (Additive) Example")
    )
display(oscillatorx1_state_plot_smooted)

For those that just want the code, without the explanation, you have come to the right place. Here it is:



In [26]:

    
using StateSpace
using Distributions
using Gadfly
using DataFrames
using Colors

Δt = 0.1 # Set the time step

#Set the process function
function processFunction(x::Vector, Δt::Float64=0.1, μ::Float64=0.3)
    x1 = zeros(x)
    x1[1] = x[1] + Δt * -x[2]
    x1[2] = x[2] + Δt * (-μ * (1 - x[1]^2) * x[2] + x[1])
    x1
end

#Set the observation function.
observationFunction(x::Vector) = x

#Set process noise covariance matrix
processCovariance = 1e-1*[0.1 0.0; 0.0 1e-3]

#Set the observation noise covariance
observationCovariance = 3e-2*eye(2)

#Create additive noise UKF model
ukfStateModel = AdditiveNonLinUKFSSM(processFunction, processCovariance, observationFunction, observationCovariance)

trueInitialState = [1.4, 0.0] #Set the true initial state of the oscillator

#Set the number of observations and generate the true values and the noisy
#observations.
numObs = 200
trueState = zeros(2,numObs)
noisyObs = zeros(2,numObs)
trueState[:,1] = trueInitialState
noisyObs[:,1] = trueInitialState + sqrt(observationCovariance)*randn(2)
for i in 2:numObs
    trueState[:,i] = processFunction(trueState[:,i-1])
    noisyObs[:,i] = trueState[:,i] + sqrt(observationCovariance)*randn(2)
end

initial_guess = MvNormal([0.0,2.0], 1.0*eye(2))

filtered_state = filter(ukfStateModel, noisyObs, initial_guess)

x_data = 1:numObs
x1_array = Vector{Float64}(numObs)
x1Var_array = Vector{Float64}(numObs)
x1_Guess = initial_guess.μ[1]
x1Var_Guess = 2*sqrt(initial_guess.Σ.mat[1,1])
for i in x_data
    current_state = filtered_state.state[i]
    x1_array[i] = current_state.μ[1]
    x1Var_array[i] = 2*sqrt(current_state.Σ.mat[1,1])
end

df_fs = DataFrame(
    x = x_data*Δt,
    y = x1_array,
    ymin = x1_array - x1Var_array,
    ymax = x1_array + x1Var_array,
    f = "Filtered values"
    )

n = 3
getColors = distinguishable_colors(n, Color[LCHab(70, 60, 240)],
                                   transform=c -> deuteranopic(c, 0.5),
                                   lchoices=Float64[65, 70, 75, 80],
                                   cchoices=Float64[0, 50, 60, 70],
                                   hchoices=linspace(0, 330, 24))

oscillatorx1_state_plot = plot(
    layer(x=x_data*Δt, y=noisyObs[1,:], Geom.point, Theme(default_color=getColors[2])),
    layer(x=x_data*Δt, y=trueState[1,:], Geom.line, Theme(default_color=getColors[3])),
    layer(df_fs, x=:x, y=:y, ymin=:ymin, ymax=:ymax, Geom.line, Geom.ribbon),
    Guide.xlabel("Time (seconds)"), Guide.ylabel("x1"),
    Guide.manual_color_key("Colour Key",["Filtered Estimate", "Measurements","True Value "],[getColors[1],getColors[2],getColors[3]]),
    Guide.title("Unscented Kalman Filter (Additive) Example")
    )
display(oscillatorx1_state_plot)

#Perform Smoothing of the filtered estimates
smoothed_state = smooth(ukfStateModel, filtered_state)

#Plot the results
x_data = 1:numObs
x1_array = Vector{Float64}(numObs)
x1Var_array = Vector{Float64}(numObs)
x1_Guess = initial_guess.μ[1]
x1Var_Guess = 2*sqrt(initial_guess.Σ.mat[1,1])
for i in x_data
    current_state = smoothed_state.state[i]
    x1_array[i] = current_state.μ[1]
    x1Var_array[i] = 2*sqrt(current_state.Σ.mat[1,1])
end
df_fs = DataFrame(
    x = x_data*Δt,
    y = x1_array,
    ymin = x1_array - x1Var_array,
    ymax = x1_array + x1Var_array,
    f = "Smoothed values"
    )

n = 3
getColors = distinguishable_colors(n, Color[LCHab(70, 60, 240)],
                                   transform=c -> deuteranopic(c, 0.5),
                                   lchoices=Float64[65, 70, 75, 80],
                                   cchoices=Float64[0, 50, 60, 70],
                                   hchoices=linspace(0, 330, 24))
oscillatorx1_state_plot_smooted = plot(
    layer(x=x_data*Δt, y=noisyObs[1,:], Geom.point, Theme(default_color=getColors[2])),
    layer(x=x_data*Δt, y=trueState[1,:], Geom.line, Theme(default_color=getColors[3])),
    layer(df_fs, x=:x, y=:y, ymin=:ymin, ymax=:ymax, Geom.line, Geom.ribbon),
    Guide.xlabel("Time (seconds)"), Guide.ylabel("x1"),
    Guide.manual_color_key("Colour Key",["Filtered Estimate", "Measurements","True Value "],[getColors[1],getColors[2],getColors[3]]),
    Guide.title("Unscented Kalman Smoother (Additive) Example")
    )
display(oscillatorx1_state_plot_smooted)

	x	y	ymin	ymax	f
1	0.1	0.0	-2.0	2.0	Filtered values
2	0.2	1.4082183222429558	1.0125689430929183	1.8038677013929934	Filtered values
3	0.30000000000000004	1.3270343378454568	0.9983130422356324	1.6557556334552812	Filtered values
4	0.4	1.3669012029391918	1.055790931877306	1.6780114740010776	Filtered values
5	0.5	1.2077546471196439	0.9022328133118163	1.5132764809274715	Filtered values
6	0.6	1.2217532103709878	0.9181560197247857	1.52535040101719	Filtered values
7	0.7000000000000001	1.031687856399231	0.7288253108774647	1.3345504019209975	Filtered values
8	0.8	1.0947832148967058	0.7921859500532789	1.3973804797401328	Filtered values
9	0.9	0.9258291881280865	0.6234388909948582	1.2282194852613149	Filtered values
10	1.0	1.0220403440150887	0.7197155623609277	1.3243651256692497	Filtered values
11	1.1	0.9238642526737051	0.6217297870966805	1.2259987182507297	Filtered values
12	1.2000000000000002	0.8123584429913062	0.5102885411473409	1.1144283448352716	Filtered values
13	1.3000000000000003	0.731448825407698	0.4293676591073239	1.033529991708072	Filtered values
14	1.4000000000000001	0.5485454062659543	0.24643641739416416	0.8506543951377443	Filtered values
15	1.5000000000000002	0.18075414491182812	-0.12151970464536577	0.483027994469022	Filtered values
16	1.6	0.26437599860254213	-0.03824756405315499	0.5669995612582392	Filtered values
17	1.7000000000000002	0.14441207634414913	-0.1582566527119613	0.4470808054002595	Filtered values
18	1.8000000000000003	-0.14837234530998245	-0.4511410116514407	0.15439632103147582	Filtered values
19	1.9000000000000001	-0.19242752586392095	-0.4954061193476691	0.11055106761982716	Filtered values
20	2.0	-0.24205808341285984	-0.5451232952211964	0.06100712839547673	Filtered values
21	2.1	-0.3822325989378521	-0.6853454681064522	-0.07911972976925197	Filtered values
22	2.2	-0.5881898289882574	-0.8913700270104367	-0.28500963096607823	Filtered values
23	2.3000000000000003	-0.5970455348178784	-0.900302233730638	-0.29378883590511873	Filtered values
24	2.4000000000000004	-0.7197779164279073	-1.023052185760254	-0.4165036470955608	Filtered values
25	2.5000000000000004	-0.7468184649139971	-1.0501156387546082	-0.4435212910733858	Filtered values
26	2.6	-0.8809464568843236	-1.1842437827708043	-0.5776491309978429	Filtered values
27	2.7	-0.9373369430642993	-1.2406418851680265	-0.6340320009605719	Filtered values
28	2.8000000000000003	-1.0442395949223962	-1.34753730399854	-0.7409418858462522	Filtered values
29	2.9000000000000004	-1.178699589832252	-1.481985554421409	-0.8754136252430953	Filtered values
30	3.0000000000000004	-1.1774740704604292	-1.4807458036131589	-0.8742023373076994	Filtered values
&vellip	&vellip	&vellip	&vellip	&vellip	&vellip