Kalman (empty) → 0.1.0.0

raw patch · 4 files changed

+138/−0 lines, 4 filesdep +basedep +hmatrixsetup-changed

Dependencies added: base, hmatrix

Files

• Kalman.cabal +23/−0
• LICENSE +30/−0
• Setup.hs +2/−0
• src/Kalman.hs +83/−0

@@ -0,0 +1,23 @@+name:                Kalman+version:             0.1.0.0+synopsis:            A slightly extended Kalman filter+homepage:            https://github.com/idontgetoutmuch/Kalman+license:             BSD3+license-file:        LICENSE+author:              Dominic Steinitz+maintainer:          dominic@steinitz.org+copyright:           Dominic Steinitz+category:            Math+build-type:          Simple+cabal-version:       >=1.10++library+  exposed-modules:     Kalman+  other-extensions:    DataKinds,+                       ScopedTypeVariables,+                       TypeOperators,+                       TypeFamilies+  build-depends:       base >=4.7 && <4.8,+                       hmatrix >=0.16 && <0.17+  hs-source-dirs:      src+  default-language:    Haskell2010

@@ -0,0 +1,30 @@+Copyright (c) 2015, Dominic Steinitz++All rights reserved.++Redistribution and use in source and binary forms, with or without+modification, are permitted provided that the following conditions are met:++    * Redistributions of source code must retain the above copyright+      notice, this list of conditions and the following disclaimer.++    * Redistributions in binary form must reproduce the above+      copyright notice, this list of conditions and the following+      disclaimer in the documentation and/or other materials provided+      with the distribution.++    * Neither the name of Dominic Steinitz nor the names of other+      contributors may be used to endorse or promote products derived+      from this software without specific prior written permission.++THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR+A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT+OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,+SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT+LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,+DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY+THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

@@ -0,0 +1,2 @@+import Distribution.Simple+main = defaultMain

@@ -0,0 +1,83 @@+{-# OPTIONS_GHC -Wall                     #-}+{-# OPTIONS_GHC -fno-warn-name-shadowing  #-}+{-# OPTIONS_GHC -fno-warn-type-defaults   #-}+{-# OPTIONS_GHC -fno-warn-unused-do-bind  #-}+{-# OPTIONS_GHC -fno-warn-missing-methods #-}+{-# OPTIONS_GHC -fno-warn-orphans         #-}++{-# LANGUAGE DataKinds                    #-}+{-# LANGUAGE ScopedTypeVariables          #-}+{-# LANGUAGE TypeOperators                #-}+{-# LANGUAGE TypeFamilies                 #-}++module Kalman (+  extKalman+  )where++import GHC.TypeLits+import Numeric.LinearAlgebra.Static hiding ( create )+import Data.Maybe ( fromJust )++-- | Take the+--+--  * Prior mean @muPrior@,+--+--  * Prior variance @sigmaPrior@,+--+--  * Observation map (represented as a matrix) @bigH@,+--+--  * Observation noise @bigSigmaY@,+--+--  * State update function @littleA@,+--+--  * A function which return the Jacobian of the state update+--  function at a given point @bigABuilder@,+--+-- * State noise @bigSigmaX@,+--+-- * List of observations @ys@+--+-- and return the posterior mean and variance.+extKalman ::  forall m n .+              (KnownNat m, KnownNat n, (1 <=? n) ~ 'True, (1 <=? m) ~ 'True) =>+              R n -> Sq n ->+              L m n -> Sq m ->+              (R n -> R n) -> (R n -> Sq n) -> Sq n ->+              [R m] ->+              [(R n, Sq n)]+extKalman muPrior sigmaPrior bigH bigSigmaY+  littleA bigABuilder bigSigmaX ys = result+  where+    result = scanl update (muPrior, sigmaPrior) ys++    update :: (R n, Sq n) -> R m -> (R n, Sq n)+    update (xHatFlat, bigSigmaHatFlat) y =+      (xHatFlatNew, bigSigmaHatFlatNew)+      where++        v :: R m+        v = y - (bigH #> xHatFlat)++        bigS :: Sq m+        bigS = bigH <> bigSigmaHatFlat <> (tr bigH) + bigSigmaY++        bigK :: L n m+        bigK = bigSigmaHatFlat <> (tr bigH) <> (inv bigS)++        xHat :: R n+        xHat = xHatFlat + bigK #> v++        bigSigmaHat :: Sq n+        bigSigmaHat = bigSigmaHatFlat - bigK <> bigS <> (tr bigK)++        bigA :: Sq n+        bigA = bigABuilder xHat++        xHatFlatNew :: R n+        xHatFlatNew = littleA xHat++        bigSigmaHatFlatNew :: Sq n+        bigSigmaHatFlatNew = bigA <> bigSigmaHat <> (tr bigA) + bigSigmaX++inv :: (KnownNat n, (1 <=? n) ~ 'True) => Sq n -> Sq n+inv m = fromJust $ linSolve m eye