summaryrefslogtreecommitdiff
path: root/packages/hmatrix/examples/kalman.hs
diff options
context:
space:
mode:
authorAlberto Ruiz <aruiz@um.es>2014-05-21 10:30:55 +0200
committerAlberto Ruiz <aruiz@um.es>2014-05-21 10:30:55 +0200
commit197e88c3b56d28840217010a2871c6ea3a4dd1a4 (patch)
tree825be9d6c9d87d23f7e5497c0133d11d52c63535 /packages/hmatrix/examples/kalman.hs
parente07c3dee7235496b71a89233106d93f6cc94ada1 (diff)
update dependencies, move examples etc
Diffstat (limited to 'packages/hmatrix/examples/kalman.hs')
-rw-r--r--packages/hmatrix/examples/kalman.hs51
1 files changed, 0 insertions, 51 deletions
diff --git a/packages/hmatrix/examples/kalman.hs b/packages/hmatrix/examples/kalman.hs
deleted file mode 100644
index 7fbe3d2..0000000
--- a/packages/hmatrix/examples/kalman.hs
+++ /dev/null
@@ -1,51 +0,0 @@
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))