| Copyright | (c) 2016 FP Complete Corporation |
|---|---|
| License | MIT (see LICENSE) |
| Maintainer | dominic@steinitz.org |
| Safe Haskell | None |
| Language | Haskell2010 |
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 |