diff options
Diffstat (limited to 'packages/hmatrix/examples/kalman.hs')
-rw-r--r-- | packages/hmatrix/examples/kalman.hs | 51 |
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 @@ | |||
1 | import Numeric.LinearAlgebra | ||
2 | import Graphics.Plot | ||
3 | |||
4 | vector l = fromList l :: Vector Double | ||
5 | matrix ls = fromLists ls :: Matrix Double | ||
6 | diagl = diag . vector | ||
7 | |||
8 | f = matrix [[1,0,0,0], | ||
9 | [1,1,0,0], | ||
10 | [0,0,1,0], | ||
11 | [0,0,0,1]] | ||
12 | |||
13 | h = matrix [[0,-1,1,0], | ||
14 | [0,-1,0,1]] | ||
15 | |||
16 | q = diagl [1,1,0,0] | ||
17 | |||
18 | r = diagl [2,2] | ||
19 | |||
20 | s0 = State (vector [0, 0, 10, -10]) (diagl [10,0, 100, 100]) | ||
21 | |||
22 | data System = System {kF, kH, kQ, kR :: Matrix Double} | ||
23 | data State = State {sX :: Vector Double , sP :: Matrix Double} | ||
24 | type Measurement = Vector Double | ||
25 | |||
26 | kalman :: System -> State -> Measurement -> State | ||
27 | kalman (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 | |||
36 | sys = System f h q r | ||
37 | |||
38 | zs = [vector [15-k,-20-k] | k <- [0..]] | ||
39 | |||
40 | xs = s0 : zipWith (kalman sys) xs zs | ||
41 | |||
42 | des = map (sqrt.takeDiag.sP) xs | ||
43 | |||
44 | evolution 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 | |||
48 | main = do | ||
49 | print $ fromRows $ take 10 (map sX xs) | ||
50 | mapM_ (print . sP) $ take 10 xs | ||
51 | mplot (evolution 20 (xs,des)) | ||