summaryrefslogtreecommitdiff
path: root/examples/kalman.hs
diff options
context:
space:
mode:
Diffstat (limited to 'examples/kalman.hs')
-rw-r--r--examples/kalman.hs51
1 files changed, 51 insertions, 0 deletions
diff --git a/examples/kalman.hs b/examples/kalman.hs
new file mode 100644
index 0000000..7fbe3d2
--- /dev/null
+++ b/examples/kalman.hs
@@ -0,0 +1,51 @@
1import Numeric.LinearAlgebra
2import Graphics.Plot
3
4vector l = fromList l :: Vector Double
5matrix ls = fromLists ls :: Matrix Double
6diagl = diag . vector
7
8f = matrix [[1,0,0,0],
9 [1,1,0,0],
10 [0,0,1,0],
11 [0,0,0,1]]
12
13h = matrix [[0,-1,1,0],
14 [0,-1,0,1]]
15
16q = diagl [1,1,0,0]
17
18r = diagl [2,2]
19
20s0 = State (vector [0, 0, 10, -10]) (diagl [10,0, 100, 100])
21
22data System = System {kF, kH, kQ, kR :: Matrix Double}
23data State = State {sX :: Vector Double , sP :: Matrix Double}
24type Measurement = Vector Double
25
26kalman :: System -> State -> Measurement -> State
27kalman (System f h q r) (State x p) z = State x' p' where
28 px = f <> x -- prediction
29 pq = f <> p <> trans f + q -- its covariance
30 y = z - h <> px -- residue
31 cy = h <> pq <> trans h + r -- its covariance
32 k = pq <> trans h <> inv cy -- kalman gain
33 x' = px + k <> y -- new state
34 p' = (ident (dim x) - k <> h) <> pq -- its covariance
35
36sys = System f h q r
37
38zs = [vector [15-k,-20-k] | k <- [0..]]
39
40xs = s0 : zipWith (kalman sys) xs zs
41
42des = map (sqrt.takeDiag.sP) xs
43
44evolution n (xs,des) =
45 vector [1.. fromIntegral n]:(toColumns $ fromRows $ take n (zipWith (-) (map sX xs) des)) ++
46 (toColumns $ fromRows $ take n (zipWith (+) (map sX xs) des))
47
48main = do
49 print $ fromRows $ take 10 (map sX xs)
50 mapM_ (print . sP) $ take 10 xs
51 mplot (evolution 20 (xs,des))