diff options
Diffstat (limited to 'examples/kalman.hs')
-rw-r--r-- | examples/kalman.hs | 33 |
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 @@ | |||
1 | import Numeric.LinearAlgebra | 1 | import Numeric.LinearAlgebra |
2 | import Graphics.Plot | 2 | import Graphics.Plot |
3 | 3 | ||
4 | vector l = fromList l :: Vector Double | 4 | f = fromLists |
5 | matrix ls = fromLists ls :: Matrix Double | 5 | [[1,0,0,0], |
6 | diagl = diag . vector | 6 | [1,1,0,0], |
7 | [0,0,1,0], | ||
8 | [0,0,0,1]] | ||
7 | 9 | ||
8 | f = matrix [[1,0,0,0], | 10 | h = 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 | |||
13 | h = matrix [[0,-1,1,0], | ||
14 | [0,-1,0,1]] | ||
15 | 13 | ||
16 | q = diagl [1,1,0,0] | 14 | q = diagl [1,1,0,0] |
17 | 15 | ||
@@ -25,13 +23,13 @@ type Measurement = Vector Double | |||
25 | 23 | ||
26 | kalman :: System -> State -> Measurement -> State | 24 | kalman :: System -> State -> Measurement -> State |
27 | kalman (System f h q r) (State x p) z = State x' p' where | 25 | kalman (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 | ||
36 | sys = System f h q r | 34 | sys = 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 | |||