kalman-1.0.0.0: src/Numeric/Particle.hs
-----------------------------------------------------------------------------
-- |
-- Module : Numeric.Particle
-- Copyright : (c) 2016 FP Complete Corporation
-- License : MIT (see LICENSE)
-- Maintainer : dominic@steinitz.org
--
-- =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](https://en.wikipedia.org/wiki/Particle_filter#Backward_particle_smoothers)
-- 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](https://en.wikipedia.org/wiki/White_noise#Mathematical_definitions)
-- is
--
-- \[
-- \frac{\mathrm{d}^2\alpha}{\mathrm{d}t^2} = -g\sin\alpha + w(t)
-- \]
--
-- We can discretize this via the usual [Euler method](https://en.wikipedia.org/wiki/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](http://www.cambridge.org/gb/academic/subjects/statistics-probability/applied-probability-and-stochastic-networks/bayesian-filtering-and-smoothing)
-- 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)
--
--
-- <<diagrams/src_Numeric_Particle_diagV.svg#diagram=diagV&height=600&width=500>>
--
-- === 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))
--
-----------------------------------------------------------------------------
{-# OPTIONS_GHC -Wall #-}
{-# LANGUAGE BangPatterns #-}
module Numeric.Particle (
runPF
, oneSmoothingPath
, Particles
, Path
) where
import Data.Random hiding ( StdNormal, Normal )
import Control.Monad
import qualified Data.Vector as V
import Data.Vector ( Vector )
import Data.Bits ( shiftR )
type Particles a = Vector a -- ^ As an aid for the reader
type Path a = Vector a -- ^ As an aid for the reader
runPF
:: 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
runPF stateUpdate obsUpdate weight statePrevs obs = do
stateNews <- stateUpdate statePrevs
let obsNews = obsUpdate stateNews
let weights = V.map (weight obs) obsNews
cumSumWeights = V.tail $ V.scanl (+) 0 weights
totWeight = V.last cumSumWeights
nParticles = V.length statePrevs
vs <- V.replicateM nParticles (sample $ uniform 0.0 totWeight)
let js = indices cumSumWeights vs
stateTildes = V.map (stateNews V.!) js
return stateTildes
oneSmoothingPath
:: 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
oneSmoothingPath stateUpdate weight nParticles filterEstss = do
let ys = filterEstss
ix <- sample $ uniform 0 (nParticles - 1)
let xn = (V.head ys) V.! ix
scanMapM (oneSmoothingStep stateUpdate weight) return xn (V.tail ys)
oneSmoothingStep :: MonadRandom m =>
(Particles a -> m (Particles a)) ->
(a -> a -> Double) ->
a ->
Particles a ->
m a
oneSmoothingStep stateUpdate
stateDensity
smoothingSampleAtiPlus1
filterSamplesAti = do it
where
it = do
mus <- stateUpdate filterSamplesAti
let weights = V.map (stateDensity smoothingSampleAtiPlus1) mus
cumSumWeights = V.tail $ V.scanl (+) 0 weights
totWeight = V.last cumSumWeights
v <- sample $ uniform 0.0 totWeight
let ix = binarySearch cumSumWeights v
xnNew = filterSamplesAti V.! ix
return $ xnNew
indices :: Vector Double -> Vector Double -> Vector Int
indices bs xs = V.map (binarySearch bs) xs
binarySearch :: (Ord a) =>
Vector a -> a -> Int
binarySearch vec x = loop 0 (V.length vec - 1)
where
loop !l !u
| u <= l = l
| otherwise = let e = vec V.! k in if x <= e then loop l k else loop (k+1) u
where k = l + (u - l) `shiftR` 1
scanMapM :: Monad m => (s -> a -> m s) -> (s -> m b) -> s -> Vector a -> m (Vector b)
scanMapM f g !s0 !xs
| V.null xs = do
r <- g s0
return $ V.singleton r
| otherwise = do
s <- f s0 (V.head xs)
r <- g s0
liftM (r `V.cons`) (scanMapM f g s (V.tail xs))