summaryrefslogtreecommitdiff
path: root/examples/kalman.hs
diff options
context:
space:
mode:
Diffstat (limited to 'examples/kalman.hs')
-rw-r--r--examples/kalman.hs33
1 files changed, 16 insertions, 17 deletions
diff --git a/examples/kalman.hs b/examples/kalman.hs
index 7fbe3d2..9756aa0 100644
--- a/examples/kalman.hs
+++ b/examples/kalman.hs
@@ -1,17 +1,15 @@
1import Numeric.LinearAlgebra 1import Numeric.LinearAlgebra
2import Graphics.Plot 2import Graphics.Plot
3 3
4vector l = fromList l :: Vector Double 4f = fromLists
5matrix ls = fromLists ls :: Matrix Double 5 [[1,0,0,0],
6diagl = diag . vector 6 [1,1,0,0],
7 [0,0,1,0],
8 [0,0,0,1]]
7 9
8f = matrix [[1,0,0,0], 10h = fromLists
9 [1,1,0,0], 11 [[0,-1,1,0],
10 [0,0,1,0], 12 [0,-1,0,1]]
11 [0,0,0,1]]
12
13h = matrix [[0,-1,1,0],
14 [0,-1,0,1]]
15 13
16q = diagl [1,1,0,0] 14q = diagl [1,1,0,0]
17 15
@@ -25,13 +23,13 @@ type Measurement = Vector Double
25 23
26kalman :: System -> State -> Measurement -> State 24kalman :: System -> State -> Measurement -> State
27kalman (System f h q r) (State x p) z = State x' p' where 25kalman (System f h q r) (State x p) z = State x' p' where
28 px = f <> x -- prediction 26 px = f #> x -- prediction
29 pq = f <> p <> trans f + q -- its covariance 27 pq = f <> p <> tr f + q -- its covariance
30 y = z - h <> px -- residue 28 y = z - h #> px -- residue
31 cy = h <> pq <> trans h + r -- its covariance 29 cy = h <> pq <> tr h + r -- its covariance
32 k = pq <> trans h <> inv cy -- kalman gain 30 k = pq <> tr h <> inv cy -- kalman gain
33 x' = px + k <> y -- new state 31 x' = px + k #> y -- new state
34 p' = (ident (dim x) - k <> h) <> pq -- its covariance 32 p' = (ident (size x) - k <> h) <> pq -- its covariance
35 33
36sys = System f h q r 34sys = System f h q r
37 35
@@ -49,3 +47,4 @@ main = do
49 print $ fromRows $ take 10 (map sX xs) 47 print $ fromRows $ take 10 (map sX xs)
50 mapM_ (print . sP) $ take 10 xs 48 mapM_ (print . sP) $ take 10 xs
51 mplot (evolution 20 (xs,des)) 49 mplot (evolution 20 (xs,des))
50