summaryrefslogtreecommitdiff
path: root/examples
diff options
context:
space:
mode:
Diffstat (limited to 'examples')
-rw-r--r--examples/kalman.hs6
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
26kalman :: System -> State -> Measurement -> State 26kalman :: System -> State -> Measurement -> State
27kalman (System f h q r) (State x p) z = State x' p' where 27kalman (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
36sys = System f h q r 36sys = 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