diff options
Diffstat (limited to 'examples')
-rw-r--r-- | examples/kalman.hs | 6 |
1 files changed, 2 insertions, 4 deletions
diff --git a/examples/kalman.hs b/examples/kalman.hs index e191cbb..7fbe3d2 100644 --- a/examples/kalman.hs +++ b/examples/kalman.hs | |||
@@ -26,12 +26,12 @@ type Measurement = Vector Double | |||
26 | kalman :: System -> State -> Measurement -> State | 26 | kalman :: System -> State -> Measurement -> State |
27 | kalman (System f h q r) (State x p) z = State x' p' where | 27 | kalman (System f h q r) (State x p) z = State x' p' where |
28 | px = f <> x -- prediction | 28 | px = f <> x -- prediction |
29 | pq = f <> p <> trans f -- its covariance | 29 | pq = f <> p <> trans f + q -- its covariance |
30 | y = z - h <> px -- residue | 30 | y = z - h <> px -- residue |
31 | cy = h <> pq <> trans h + r -- its covariance | 31 | cy = h <> pq <> trans h + r -- its covariance |
32 | k = pq <> trans h <> inv cy -- kalman gain | 32 | k = pq <> trans h <> inv cy -- kalman gain |
33 | x' = px + k <> y -- new state | 33 | x' = px + k <> y -- new state |
34 | p' = (ident (dim x) - k <> h) <> pq -- its covariance | 34 | p' = (ident (dim x) - k <> h) <> pq -- its covariance |
35 | 35 | ||
36 | sys = System f h q r | 36 | sys = System f h q r |
37 | 37 | ||
@@ -49,5 +49,3 @@ main = do | |||
49 | print $ fromRows $ take 10 (map sX xs) | 49 | print $ fromRows $ take 10 (map sX xs) |
50 | mapM_ (print . sP) $ take 10 xs | 50 | mapM_ (print . sP) $ take 10 xs |
51 | mplot (evolution 20 (xs,des)) | 51 | mplot (evolution 20 (xs,des)) |
52 | |||
53 | --(<>) = multiply \ No newline at end of file | ||