kalman-1.0.0.2: Kalman and particle filters and smoothers

Numeric.Particle

Description

# The Theory

The particle filter, runPF, in this library is applicable to the state space model given by

\begin{aligned} \boldsymbol{x}_i &= \boldsymbol{a}_i(\boldsymbol{x}_{i-1}) + \boldsymbol{\psi}_{i-1} \\ \boldsymbol{y}_i &= \boldsymbol{h}_i(\boldsymbol{x}_i) + \boldsymbol{\upsilon}_i \end{aligned}

where

• $$\boldsymbol{a_i}$$ is some non-linear vector-valued possibly time-varying state update function.
• $$\boldsymbol{\psi}_{i}$$ are independent normally distributed random variables with mean 0 representing the fact that the state update is noisy: $$\boldsymbol{\psi}_{i} \sim {\cal{N}}(0,Q_i)$$.
• $$\boldsymbol{h}_i$$ is some non-linear vector-valued possibly time-varying function describing how we observe the hidden state process.
• $$\boldsymbol{\upsilon}_i$$ are independent normally distributed random variables with mean 0 represent the fact that the observations are noisy: $$\boldsymbol{\upsilon}_{i} \sim {\cal{N}}(0,R_i)$$.

Clearly this could be generalised further; anyone wishing for such a generalisation is encouraged to contribute to the library.

The smoother, oneSmoothingPath implements Forward filtering / backward smoothing and returns just one path from the particle filter; in most cases, this will need to be run many times to provide good estimates of the past. Note that oneSmoothingPath uses all observation up until the current time. This could be generalised to select only a window of observations up to the current time. Again, contributions to implement this generalisation are welcomed.

# An Extended Example

The equation of motion for a pendulum of unit length subject to Gaussian white noise is

$\frac{\mathrm{d}^2\alpha}{\mathrm{d}t^2} = -g\sin\alpha + w(t)$

We can discretize this via the usual Euler method

$\begin{bmatrix} x_{1,i} \\ x_{2,i} \end{bmatrix} = \begin{bmatrix} x_{1,i-1} + x_{2,i-1}\Delta t \\ x_{2,i-1} - g\sin x_{1,i-1}\Delta t \end{bmatrix} + \mathbf{q}_{i-1}$

where $$q_i \sim {\mathcal{N}}(0,Q)$$ and

$Q = \begin{bmatrix} \frac{q^c \Delta t^3}{3} & \frac{q^c \Delta t^2}{2} \\ \frac{q^c \Delta t^2}{2} & {q^c \Delta t} \end{bmatrix}$

Assume that we can only measure the horizontal position of the pendulum and further that this measurement is subject to error so that

$y_i = \sin x_i + r_i$

where $$r_i \sim {\mathcal{N}}(0,R)$$.

First let's set the time step and the acceleration caused by earth's gravity and the covariance matrices for the state and observation.

{-# LANGUAGE DataKinds #-}

import Numeric.Particle

deltaT, g :: Double
deltaT = 0.01
g      = 9.81

bigQ :: Sym 2
bigQ = sym $matrix bigQl qc :: Double qc = 0.01 bigQl :: [Double] bigQl = [ qc * deltaT^3 / 3, qc * deltaT^2 / 2, qc * deltaT^2 / 2, qc * deltaT ] bigR :: Sym 1 bigR = sym$ matrix [0.2]

data SystemState a = SystemState { x1  :: a, x2  :: a }
deriving Show

newtype SystemObs a = SystemObs { y1  :: a }
deriving Show


We use the data generated using code made available with Simo Särkkä's book which starts with an angle of 1.5 radians and an angular velocity of 0.0 radians per second. We set our prior to have a mean of 1.6, not too far from the actual value.

{-# LANGUAGE DataKinds #-}

m0 :: PendulumState
m0 = vector [1.6, 0]

bigP :: Sym 2
bigP = sym $diag 0.1 initParticles :: R.MonadRandom m => m (Particles (SystemState Double)) initParticles = V.replicateM nParticles$ do
r <- R.sample $R.rvar (Normal m0 bigP) let x1 = fst$ headTail r
x2 = fst $headTail$ snd $headTail r return$ SystemState { x1 = x1, x2 = x2}

nObs :: Int
nObs = 200

nParticles :: Int
nParticles = 20

(.+), (.*), (.-) :: (Num a) => V.Vector a -> V.Vector a -> V.Vector a
(.+) = V.zipWith (+)
(.*) = V.zipWith (*)
(.-) = V.zipWith (-)

stateUpdateP :: Particles (SystemState Double) ->
Particles (SystemState Double)
stateUpdateP xPrevs = V.zipWith SystemState x1s x2s
where
ix = V.length xPrevs

x1Prevs = V.map x1 xPrevs
x2Prevs = V.map x2 xPrevs

deltaTs = V.replicate ix deltaT
gs = V.replicate ix g
x1s = x1Prevs .+ (x2Prevs .* deltaTs)
x2s = x2Prevs .- (gs .* (V.map sin x1Prevs) .* deltaTs) ### Code for Plotting

The full code for plotting the results:

{-# LANGUAGE ExplicitForAll #-}
{-# LANGUAGE TypeOperators  #-}
{-# LANGUAGE DataKinds      #-}

import qualified Graphics.Rendering.Chart as C
import Graphics.Rendering.Chart.Backend.Diagrams
import Data.Colour
import Data.Colour.Names
import Data.Default.Class
import Control.Lens

import Data.Csv
import System.IO hiding ( hGetContents )
import Data.ByteString.Lazy ( hGetContents )
import qualified Data.Vector as V

import           Data.Random.Distribution.Static.MultivariateNormal ( Normal(..) )
import qualified Data.Random as R
import           Data.Random.Source.PureMT ( pureMT )
import           Control.Monad.State ( evalState, replicateM )

import Data.List ( transpose )

import GHC.TypeLits ( KnownNat )

import Numeric.LinearAlgebra.Static

chartEstimated :: String ->
[(Double, Double)] ->
[(Double, Double)] ->
[(Double, Double)] ->
C.Renderable ()
chartEstimated title acts obs ests = C.toRenderable layout
where

actuals = C.plot_lines_values .~ [acts]
$C.plot_lines_style . C.line_color .~ opaque red$ C.plot_lines_title .~ "Actual Trajectory"
$C.plot_lines_style . C.line_width .~ 1.0$ def

measurements = C.plot_points_values .~ obs
$C.plot_points_style . C.point_color .~ opaque blue$ C.plot_points_title .~ "Measurements"
$def estimas = C.plot_lines_values .~ [ests]$ C.plot_lines_style  . C.line_color .~ opaque black
$C.plot_lines_title .~ "Inferred Trajectory"$ C.plot_lines_style  . C.line_width .~ 1.0
$def layout = C.layout_title .~ title$ C.layout_plots .~ [C.toPlot actuals, C.toPlot measurements, C.toPlot estimas]
$C.layout_y_axis . C.laxis_title .~ "Angle / Horizontal Displacement"$ C.layout_y_axis . C.laxis_override .~ C.axisGridHide
$C.layout_x_axis . C.laxis_title .~ "Time"$ C.layout_x_axis . C.laxis_override .~ C.axisGridHide
$def stateUpdateNoisy :: R.MonadRandom m => Sym 2 -> Particles (SystemState Double) -> m (Particles (SystemState Double)) stateUpdateNoisy bigQ xPrevs = do let xs = stateUpdateP xPrevs x1s = V.map x1 xs x2s = V.map x2 xs let ix = V.length xPrevs etas <- replicateM ix$ R.sample $R.rvar (Normal 0.0 bigQ) let eta1s, eta2s :: V.Vector Double eta1s = V.fromList$ map (fst . headTail) etas
eta2s = V.fromList $map (fst . headTail . snd . headTail) etas return (V.zipWith SystemState (x1s .+ eta1s) (x2s .+ eta2s)) obsUpdate :: Particles (SystemState Double) -> Particles (SystemObs Double) obsUpdate xs = V.map (SystemObs . sin . x1) xs weight :: forall a n . KnownNat n => (a -> R n) -> Sym n -> a -> a -> Double weight f bigR obs obsNew = R.pdf (Normal (f obsNew) bigR) (f obs) runFilter :: Particles (SystemObs Double) -> V.Vector (Particles (SystemState Double)) runFilter pendulumSamples = evalState action (pureMT 19) where action = do xs <- initParticles scanMapM (runPF (stateUpdateNoisy bigQ) obsUpdate (weight f bigR)) return xs pendulumSamples testSmoothing :: Particles (SystemObs Double) -> Int -> [Double] testSmoothing ss n = V.toList$ evalState action (pureMT 23)
where
action = do
xss <- V.replicateM n $oneSmoothingPath (stateUpdateNoisy bigQ) (weight h bigQ) nParticles (runFilter ss) let yss = V.fromList$ map V.fromList $transpose$
V.toList $V.map (V.toList)$
xss
return $V.map (/ (fromIntegral n))$ V.map V.sum $V.map (V.map x1) yss type PendulumState = R 2 f :: SystemObs Double -> R 1 f = vector . pure . y1 h :: SystemState Double -> R 2 h u = vector [x1 u , x2 u] diagV = do h <- openFile "data/matlabRNGs.csv" ReadMode cs <- hGetContents h let df = (decode NoHeader cs) :: Either String (V.Vector (Double, Double)) case df of Left _ -> error "Whatever" Right generatedSamples -> do let preObs = V.take nObs$ V.map fst generatedSamples
let obs = V.toList preObs
let acts = V.toList $V.take nObs$ V.map snd generatedSamples
let nus = take nObs (testSmoothing (V.map SystemObs preObs) 50)
denv <- defaultEnv C.vectorAlignmentFns 600 500
let charte = chartEstimated "Particle Smoother"
(zip [0,1..] acts)
(zip [0,1..] obs)
(zip [0,1..] nus)
return $fst$ runBackend denv (C.render charte (600, 500))

# Documentation

Arguments

 :: MonadRandom m => (Particles a -> m (Particles a)) System evolution at a point $$\boldsymbol{a}_i(\boldsymbol{x})$$ -> (Particles a -> Particles b) Measurement operator at a point $$\boldsymbol{h}_i$$ -> (b -> b -> Double) Observation probability density function $$p(\boldsymbol{y}_k|\boldsymbol{x}^{(i)}_k)$$ -> Particles a Current estimate $$\big\{\big(w^{(i)}_k, \boldsymbol{x}^{(i)}_k\big) : i \in \{1,\ldots,N\}\big\}$$ where $$N$$ is the number of particles -> b New measurement $$\boldsymbol{y}_i$$ -> m (Particles a) New estimate $$\big\{\big(w^{(i)}_{k+1}, \boldsymbol{x}^{(i)}_{k+1}\big) : i \in \{1,\ldots,N\}\big\}$$ where $$N$$ is the number of particles

Arguments

 :: MonadRandom m => (Particles a -> m (Particles a)) System evolution at a point $$\boldsymbol{a}_i(\boldsymbol{x})$$ -> (a -> a -> Double) State probability density function $$p(\boldsymbol{x}^{(i)}_k|\boldsymbol{x}^{(i)}_{k-1})$$ -> Int Number of particles $$N$$ -> Vector (Particles a) Filtering estimates $$\bigg\{\big\{\big(w^{(i)}_{k+1}, \boldsymbol{x}^{(i)}_{k+1}\big) : i \in \{1,\ldots,N\}\big\} : k \in \{1,\ldots,T\}\bigg\}$$ where $$T$$ is the number of timesteps -> m (Path a) A path for the smoothed particle $$\{\boldsymbol{x}_k : k \in T\}$$ where $$T$$ is the number of timesteps

type Particles a Source #

Arguments

 = Vector a As an aid for the reader

type Path a Source #

Arguments

 = Vector a As an aid for the reader