Merge pull request #295 from LAMPSPUC/gb/veh-track2

Vehicle Tracking model

Author: guilhermebodin

docs/src/examples.md

@@ -193,6 +193,44 @@ smoother_output = kalman_smoother(model)
 plot(df.date, get_smoothed_state(smoother_output)[:, 2], label = "slope")
 ```
 
+## Vehicle tracking
+
+This example illustrates how to perform vehicle tracking from noisy data.
+
+```@setup bt
+using StateSpaceModels, Random
+using Plots
+```
+
+```@example vehicle_tracking
+using Random
+Random.seed!(1)
+
+# Define a random trajectory
+n = 100
+H = [1 0; 0 1.0]
+Q = [1 0; 0 1.0]
+rho = 0.1
+model = VehicleTracking(rand(n, 2), rho, H, Q)
+initial_state = [0.0, 0, 0, 0]
+sim = StateSpaceModels.simulate(model.system, initial_state, n)
+
+# Use a Kalman filter to get the predictive and filtered states
+model = VehicleTracking(sim, 0.1, H, Q)
+kalman_filter(model)
+pos_pred = get_predictive_state(model)
+pos_filtered = get_filtered_state(model)
+
+# Plot a gif illustrating the result
+using Plots
+anim = @animate for i in 1:n
+    plot(sim[1:i, 1], sim[1:i, 2], label="Measured position", line=:scatter, lw=2, markeralpha=0.2, color=:black, title="Vehicle tracking")
+    plot!(pos_pred[1:i+1, 1], pos_pred[1:i+1, 3], label = "Predicted position", lw=2, color=:forestgreen)
+    plot!(pos_filtered[1:i, 1], pos_filtered[1:i, 3], label = "Filtered position", lw=2, color=:indianred)
+end
+gif(anim, "anim_fps15.gif", fps = 15)
+```
+
 ## Cross validation of the forecasts of a model
 
 Often times users would like to compare the forecasting skill of different models. The function 

docs/src/manual.md

@@ -55,6 +55,7 @@ LocalLevelCycle
 LocalLevelExplanatory
 LocalLinearTrend
 MultivariateBasicStructural
+VehicleTracking
 ```
 
 ## Naive models

src/StateSpaceModels.jl

@@ -53,6 +53,7 @@ include("models/unobserved_components.jl")
 include("models/exponential_smoothing.jl")
 include("models/naive_models.jl")
 include("models/dar.jl")
+include("models/vehicle_tracking.jl")
 
 include("visualization/forecast.jl")
 include("visualization/components.jl")
@@ -86,6 +87,7 @@ export SparseUnivariateKalmanFilter
 export StateSpaceModel
 export UnivariateKalmanFilter
 export UnobservedComponents
+export VehicleTracking
 
 # Exported functions
 export auto_arima

src/models/locallineartrend.jl

@@ -1,4 +1,6 @@
 @doc raw"""
+    LocalLinearTrend(y::Vector{Fl}) where Fl
+
 The linear trend model is defined by:
 ```math
 \begin{gather*}

src/models/vehicle_tracking.jl

@@ -0,0 +1,59 @@
+@doc raw"""
+    VehicleTracking(y::Matrix{Fl}, ρ::Fl, H::Matrix{Fl}, Q::Matrix{Fl}) where Fl
+
+The vehicle tracking example illustrates a model where there are no hyperparameters, 
+the user defines the parameters ``\rho``, ``H`` and ``Q`` and the model gives the predicted and filtered speed and
+position. In this case, $y_t$ is a $2 \times 1$ observation vector representing the corrupted measurements 
+of the vehicle's position on the two-dimensional plane in instant $t$.
+
+The position and speed in each dimension compose the state of the vehicle. Let us refer to ``x_t^{(d)}`` as 
+the position on the axis $d$ and to ``\dot{x}^{(d)}_t`` as the speed on the axis $d$ in instant ``t``. Additionally, 
+let ``\eta^{(d)}_t`` be the input drive force on the axis ``d``, which acts as state noise. For a single dimension, 
+we can describe the vehicle dynamics as 
+
+```math
+\begin{equation}
+    \begin{aligned}
+        & x_{t+1}^{(d)} = x_t^{(d)} + \Big( 1 - \frac{\rho \Delta_t}{2} \Big) \Delta_t \dot{x}^{(d)}_t + \frac{\Delta^2_t}{2} \eta_t^{(d)}, \\
+        & \dot{x}^{(d)}_{t+1} = (1 - \rho) \dot{x}^{(d)}_{t} + \Delta_t \eta^{(d)}_t,
+    \end{aligned}\label{eq_control}
+\end{equation}
+```
+
+We can cast the dynamical system as a state-space model in the following manner:
+```math
+\begin{align*} 
+    y_t &= \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 \end{bmatrix} \alpha_{t+1} + \varepsilon_t, \\
+    \alpha_{t+1} &= \begin{bmatrix} 1 & (1 - \tfrac{\rho \Delta_t}{2}) \Delta_t & 0 & 0 \\ 0 & (1 - \rho) & 0 & 0 \\ 0 & 0 & 1 & (1 - \tfrac{\rho \Delta_t}{2}) \\ 0 & 0 & 0 & (1 - \rho) \end{bmatrix} \alpha_{t} + \begin{bmatrix} \tfrac{\Delta^2_t}{2} & 0 \\ \Delta_t & 0 \\ 0 & \tfrac{\Delta^2_t}{2} \\ 0 & \Delta_t \end{bmatrix} \eta_{t},
+\end{align*}
+```
+
+See more on [Vehicle tracking](@ref)
+"""
+mutable struct VehicleTracking <: StateSpaceModel
+    system::LinearMultivariateTimeInvariant
+
+    function VehicleTracking(y::Matrix{Fl}, ρ::Fl, H::Matrix{Fl}, Q::Matrix{Fl}) where Fl
+        p = 2
+        Z = kron(Matrix{Fl}(I, p, p), [1.0 0.0])
+        T = kron(Matrix{Fl}(I, p, p), [1 (1 - ρ / 2); 0 (1 - ρ)])
+        R = kron(Matrix{Fl}(I, p, p), [0.5; 1])
+        d = zeros(Fl, p)
+        c = zeros(Fl, 4)
+        H = H
+        Q = Q
+
+        system = LinearMultivariateTimeInvariant{Fl}(y, Z, T, R, d, c, H, Q)
+
+        return new(system)
+    end
+end
+
+function default_filter(model::VehicleTracking)
+    Fl = typeof_model_elements(model)
+    steadystate_tol = Fl(1e-5)
+    a1 = zeros(Fl, num_states(model))
+    skip_llk_instants = length(a1)
+    P1 = Fl(1e6) .* Matrix{Fl}(I, num_states(model), num_states(model))
+    return MultivariateKalmanFilter(2, a1, P1, skip_llk_instants, steadystate_tol)
+end

test/models/vehicle_tracking.jl

@@ -0,0 +1,21 @@
+@testset "Vehicle tracking" begin
+    n = 100
+    H = [1 0
+         0 1.0]
+    Q = [1 0
+        0 1.0]
+    rho = 0.1
+    model = VehicleTracking(rand(n, 2), rho, H, Q)
+
+    # Not possible to fit
+    @test !has_fit_methods(VehicleTracking)
+    
+    # Simply test if it runs
+    initial_state = [0.0, 0, 0, 0]
+    sim = simulate(model.system, initial_state, n)
+    
+    model = VehicleTracking(sim, 0.1, H, Q)
+    kalman_filter(model)
+    pos_pred = get_predictive_state(model)
+    pos_filtered = get_filtered_state(model)
+end

test/runtests.jl

@@ -27,6 +27,7 @@ include("models/linear_regression.jl")
 include("models/exponential_smoothing.jl")
 include("models/naive_models.jl")
 include("models/dar.jl")
+include("models/vehicle_tracking.jl")
 
 # Visualization
 include("visualization/forecast.jl")