import Numeric.LinearAlgebra import Graphics.Plot vector l = fromList l :: Vector Double matrix ls = fromLists ls :: Matrix Double diagl = diag . vector f = matrix [[1,0,0,0], [1,1,0,0], [0,0,1,0], [0,0,0,1]] h = matrix [[0,-1,1,0], [0,-1,0,1]] q = diagl [1,1,0,0] r = diagl [2,2] s0 = State (vector [0, 0, 10, -10]) (diagl [10,0, 100, 100]) data System = System {kF, kH, kQ, kR :: Matrix Double} data State = State {sX :: Vector Double , sP :: Matrix Double} type Measurement = Vector Double kalman :: System -> State -> Measurement -> State kalman (System f h q r) (State x p) z = State x' p' where px = f <> x -- prediction pq = f <> p <> trans f + q -- its covariance y = z - h <> px -- residue cy = h <> pq <> trans h + r -- its covariance k = pq <> trans h <> inv cy -- kalman gain x' = px + k <> y -- new state p' = (ident (dim x) - k <> h) <> pq -- its covariance sys = System f h q r zs = [vector [15-k,-20-k] | k <- [0..]] xs = s0 : zipWith (kalman sys) xs zs des = map (sqrt.takeDiag.sP) xs evolution n (xs,des) = vector [1.. fromIntegral n]:(toColumns \$ fromRows \$ take n (zipWith (-) (map sX xs) des)) ++ (toColumns \$ fromRows \$ take n (zipWith (+) (map sX xs) des)) main = do print \$ fromRows \$ take 10 (map sX xs) mapM_ (print . sP) \$ take 10 xs mplot (evolution 20 (xs,des))