estimator (empty) → 1.0.0

raw patch · 13 files changed

+912/−0 lines, 13 filesdep +addep +basedep +distributivesetup-changed

Dependencies added: ad, base, distributive, lens, linear, reflection

Files

• LICENSE +30/−0
• Setup.hs +2/−0
• estimator.cabal +61/−0
• src/Numeric/Estimator.hs +18/−0
• src/Numeric/Estimator/Augment.hs +82/−0
• src/Numeric/Estimator/Class.hs +103/−0
• src/Numeric/Estimator/KalmanFilter.hs +89/−0
• src/Numeric/Estimator/Matrix.hs +50/−0
• src/Numeric/Estimator/Model/Coordinate.hs +78/−0
• src/Numeric/Estimator/Model/Pressure.hs +26/−0
• src/Numeric/Estimator/Model/SensorFusion.hs +302/−0
• src/Numeric/Estimator/Model/Symbolic.hs +30/−0
• src/Numeric/Estimator/Quaternion.hs +41/−0

@@ -0,0 +1,30 @@+Copyright (c) 2014, Galois, Inc++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 Galois, Inc 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,61 @@+name:                estimator+version:             1.0.0+synopsis:            State-space estimation algorithms such as Kalman Filters+description:+  The goal of this library is to simplify implementation and use of+  state-space estimation algorithms, such as Kalman Filters. The+  interface for constructing models is isolated as much as possible from+  the specifics of a given algorithm, so swapping out a Kalman Filter+  for a Bayesian Particle Filter should involve a minimum of effort.+  .+  This implementation is designed to support symbolic types, such as+  from <http://hackage.haskell.org/package/sbv sbv> or+  <http://hackage.haskell.org/package/ivory ivory>. As a result you can+  generate code in another language, such as C, from a model written+  using this package; or run static analyses on your model.+  .+  Also included is a sophisticated sensor fusion example in+  "Numeric.Estimator.Model.SensorFusion", which may be useful in its own+  right.+license:             BSD3+license-file:        LICENSE+author:              Jamey Sharp+maintainer:          jamey@galois.com+copyright:           2014 Galois, Inc.+homepage:            https://github.com/GaloisInc/estimator+bug-reports:         https://github.com/GaloisInc/estimator/issues+category:            Math, Numerical, Statistics+build-type:          Simple+cabal-version:       >=1.10++source-repository    this+  type:     git+  location: https://github.com/GaloisInc/estimator+  tag:      1.0.0++Flag werror+  description: Make warnings errors+  default: False++library+  hs-source-dirs:      src+  exposed-modules:     Numeric.Estimator,+                       Numeric.Estimator.Augment,+                       Numeric.Estimator.Class,+                       Numeric.Estimator.KalmanFilter,+                       Numeric.Estimator.Model.Coordinate,+                       Numeric.Estimator.Model.Pressure,+                       Numeric.Estimator.Model.SensorFusion,+                       Numeric.Estimator.Model.Symbolic+  other-modules:       Numeric.Estimator.Matrix,+                       Numeric.Estimator.Quaternion+  build-depends:       base >=4.6 && <5,+                       ad >=4.2,+                       distributive >=0.4,+                       lens >=4.6,+                       linear >=1.15,+                       reflection >=1.5+  default-language:    Haskell2010+  ghc-options:         -Wall+  if flag(werror)+    ghc-options:       -Werror

@@ -0,0 +1,18 @@+{- |+Description: Re-export the common estimator modules++System models using this package will usually require these modules.+-}++module Numeric.Estimator (+  -- * Generic types+  module Numeric.Estimator.Class,+  -- * Implementation of the Kalman Filter family of algorithms+  module Numeric.Estimator.KalmanFilter,+  -- * Support for augmented process models+  module Numeric.Estimator.Augment+) where++import Numeric.Estimator.Augment+import Numeric.Estimator.Class+import Numeric.Estimator.KalmanFilter

@@ -0,0 +1,82 @@+{-# LANGUAGE FlexibleContexts #-}+{-# LANGUAGE TypeFamilies #-}++{- |+Description: Helper to augment a process model++Some system models are best handled by injecting some measurements into+the process model. These measurements are not truly part of the filter+state, and so shouldn't appear in the state vector. However, when the+process model runs, the state needs to be augmented with these+measurements, and the process uncertainty needs to be augmented with+their noise covariance.++As currently implemented, this only works for process models where the+'Filter' type is a 'GaussianFilter' instance. Generalizing this+interface would be useful future work.+-}++module Numeric.Estimator.Augment (+  AugmentState(..), augmentProcess+) where++import Control.Applicative+import Data.Distributive+import Data.Foldable+import Data.Traversable+import Linear+import Numeric.Estimator.Class++-- | Holder for the basic state vector plus the augmented extra state.+data AugmentState state extra a = AugmentState { getState :: state a, getExtra :: extra a }++instance (Applicative state, Applicative extra) => Additive (AugmentState state extra) where+  zero = pure 0++instance (Applicative state, Applicative extra) => Applicative (AugmentState state extra) where+  pure v = AugmentState (pure v) (pure v)+  v1 <*> v2 = AugmentState+    { getState = getState v1 <*> getState v2+    , getExtra = getExtra v1 <*> getExtra v2+    }++instance (Applicative state, Applicative extra) => Functor (AugmentState state extra) where+  fmap = liftA++instance (Applicative state, Applicative extra, Traversable state, Traversable extra) => Foldable (AugmentState state extra) where+  foldMap = foldMapDefault++instance (Applicative state, Applicative extra, Traversable state, Traversable extra) => Traversable (AugmentState state extra) where+  sequenceA v = AugmentState+    <$> sequenceA (getState v)+    <*> sequenceA (getExtra v)++instance (Applicative state, Applicative extra, Distributive state, Distributive extra) => Distributive (AugmentState state extra) where+  distribute f = AugmentState+    { getState = distribute $ fmap getState f+    , getExtra = distribute $ fmap getExtra f+    }++augment2D :: (Applicative state, Applicative extra, Num a) => extra (extra a) -> state (state a) -> AugmentState state extra (AugmentState state extra a)+augment2D lr ul = AugmentState (liftA2 AugmentState ul (pure (pure 0))) (liftA2 AugmentState (pure (pure 0)) lr)++-- | Run an augmented process model with the given extra data.+augmentProcess :: (Num (Var t), Applicative state, Applicative extra, Process t, GaussianFilter (Filter t), State t ~ AugmentState state extra)+               => t+               -- ^ base process model+               -> extra (Var t)+               -- ^ extra state+               -> state (state (Var t))+               -- ^ base process uncertainty+               -> extra (extra (Var t))+               -- ^ extra process uncertainty+               -> Filter t state (Var t)+               -- ^ prior (unaugmented) state+               -> Filter t state (Var t)+               -- ^ posterior (unaugmented) state+augmentProcess model extraState noise extraNoise prior = posterior+  where+  augmentedNoise = augment2D (pure (pure 0)) noise+  augmentedPrior = mapStatistics (flip AugmentState extraState) (augment2D extraNoise) prior+  augmentedPosterior = process model augmentedNoise augmentedPrior+  posterior = mapStatistics getState (fmap getState . getState) augmentedPosterior

@@ -0,0 +1,103 @@+{-# LANGUAGE ConstraintKinds #-}+{-# LANGUAGE TypeFamilies #-}++{- |+Description: Type-classes for state-space estimation algorithms++These type classes abstract many details of estimation algorithms,+making it easier to try different algorithms while changing the model as+little as possible.++This interface does make the simplifying assumption that process+uncertainty and measurement noise are each always specified as a+covariance matrix describing a zero-mean multi-variate normal+distribution. While some estimation algorithms (such as the Bayesian+Particle Filter) can accomodate more sophisticated distributions, it's+unusual to encounter problems that require that degree of flexibility.+-}++module Numeric.Estimator.Class where++-- | An estimator is a model of a system, describing how to update a+-- prior estimated state with new information. Two kinds of estimators+-- are the 'Process' model, and the 'Measure' (or observation) model.+class Estimator t where+  -- | The type of data that this estimator maintains across updates.+  type Filter t :: (* -> *) -> * -> *++-- | The type of state vector used in an estimator.+type family State t :: * -> *++-- | The type of individual state variables used in an estimator.+type family Var t++-- | A process model updates the estimated state by predicting how the+-- system should have changed since the last prediction.+--+-- In a kinematic model, for instance, the process model might be a+-- dead-reckoning physics simulation which updates position using a+-- trivial numeric integration of velocity.+--+-- Parameter estimation problems, where the parameters are expected to+-- remain constant between observations, needn't have a process model.+class Estimator t => Process t where+  process :: t+          -- ^ process model+          -> State t (State t (Var t))+          -- ^ process uncertainty covariance+          -> Filter t (State t) (Var t)+          -- ^ prior state+          -> Filter t (State t) (Var t)+          -- ^ posterior state++-- | A measurement, or observation, model updates the estimated state+-- using some observation of the real state.+--+-- In a navigation problem, for instance, an observation might come from+-- a GPS receiver or a pressure altimeter. The model computes what value+-- the sensor would be expected to read if there were no sensor noise+-- and the current estimated state were exactly correct. The difference+-- between the expected and actual measurement is called the+-- \"innovation\", and that difference drives the estimated state toward+-- the true state.+--+-- In general, an observation is vector-valued. You can wrap up scalar+-- observations in a singleton functor, such as 'V1'.+--+-- For each dimension of the observation vector, the measurement must+-- consist of a scalar measurement, and an expression which evaluates to+-- the expected value for that measurement given the current state.+class Estimator t => Measure t where+  -- | Some estimators can compute some indication of how plausible an+  -- observation is, such as, for example, the innovation. This is the+  -- type of that quality indication, which may be @()@ if the chosen+  -- algorithm can't report measurement quality.+  type MeasureQuality t obs++  -- | An algorithm may have specific constraints on what types of+  -- observation it can process. This type has a 'Constraint' kind and+  -- captures any required type-class constraints.+  type MeasureObservable t obs++  measure :: MeasureObservable t obs+          => obs (Var t, t)+          -- ^ measurement model+          -> obs (obs (Var t))+          -- ^ measurement noise covariance+          -> Filter t (State t) (Var t)+          -- ^ prior state+          -> (MeasureQuality t obs, Filter t (State t) (Var t))+          -- ^ measurement quality and posterior state++-- | A filter whose state can be captured as a multi-variate normal+-- distribution can also be updated by adjusting the parameters of that+-- distribution.+class GaussianFilter t where+  mapStatistics :: (state var -> state' var')+                -- ^ update mean+                -> (state (state var) -> state' (state' var'))+                -- ^ update covariance+                -> t state var+                -- ^ original state+                -> t state' var'+                -- ^ updated state

@@ -0,0 +1,89 @@+{-# LANGUAGE FlexibleContexts #-}+{-# LANGUAGE Rank2Types #-}+{-# LANGUAGE TypeFamilies #-}++{- |+Description: Kalman Filter estimator algorithm++This module implements the Extended Kalman Filter estimation algorithm.+-}++module Numeric.Estimator.KalmanFilter where++import Data.Distributive+import Data.Reflection (Reifies)+import Data.Traversable+import Linear+import Numeric.AD.Internal.Reverse (Tape)+import Numeric.AD.Mode.Reverse+import Numeric.Estimator.Class+import Numeric.Estimator.Matrix++-- | All variants of Kalman Filter, at their core, maintain the+-- parameters of a multi-variate normal distribution.+--+-- Since different Kalman Filter variants share this filter type, you+-- can mix and match algorithms within the same filter. For example, you+-- could use a conventional Kalman filter for any linear measurements,+-- and a Sigma-Point Kalman Filter for a non-linear process model.+data KalmanFilter state var = KalmanFilter+  { kalmanState :: state var -- ^ mean+  , kalmanCovariance :: state (state var) -- ^ covariance+  }++type instance State (KalmanFilter state var) = state+type instance Var (KalmanFilter state var) = var++instance GaussianFilter KalmanFilter where+  mapStatistics mapState mapCov (KalmanFilter state cov) = KalmanFilter (mapState state) (mapCov cov)++-- | Kalman filter estimators can report the innovation of each+-- observation, as well as the covariance of the innovation.+data KalmanInnovation obs var = KalmanInnovation+  { kalmanInnovation :: obs var+  , kalmanInnovationCovariance :: obs (obs var)+  }++-- | A process model in an Extended Kalman Filter transforms a state+-- vector to a new state vector, but is wrapped in reverse-mode+-- automatic differentiation.+newtype EKFProcess state var = EKFProcess (forall s. Reifies s Tape => state (Reverse s var) -> state (Reverse s var))++type instance State (EKFProcess state var) = state+type instance Var (EKFProcess state var) = var++instance Estimator (EKFProcess state var) where+  type Filter (EKFProcess state var) = KalmanFilter++instance (Additive state, Traversable state, Distributive state, Num var) => Process (EKFProcess state var) where+  process (EKFProcess model) q prior = KalmanFilter state' (q !+! f !*! kalmanCovariance prior !*! transpose f)+    where+    predicted = jacobian' model $ kalmanState prior+    state' = fmap fst predicted+    f = fmap snd predicted++-- | A measurement model in an Extended Kalman Filter uses the state+-- vector to predict what value a sensor should return, while wrapped in+-- reverse-mode automatic differentiation.+newtype EKFMeasurement state var = EKFMeasurement (forall s. Reifies s Tape => state (Reverse s var) -> Reverse s var)++type instance State (EKFMeasurement state var) = state+type instance Var (EKFMeasurement state var) = var++instance Estimator (EKFMeasurement state var) where+  type Filter (EKFMeasurement state var) = KalmanFilter++instance (Additive state, Distributive state, Traversable state, Fractional var) => Measure (EKFMeasurement state var) where+  type MeasureQuality (EKFMeasurement state var) obs = KalmanInnovation obs var+  type MeasureObservable (EKFMeasurement state var) obs = (Additive obs, Traversable obs)++  measure measurements obsCov prior = (KalmanInnovation innovation innovCov, KalmanFilter state' errorCov')+    where+    predicted = jacobian' (\ stateVars -> fmap (\ (_, EKFMeasurement h) -> h stateVars) measurements) (kalmanState prior)+    innovation = fmap fst measurements ^-^ fmap fst predicted+    obsModel = fmap snd predicted+    ph = kalmanCovariance prior !*! transpose obsModel+    innovCov = obsCov !+! obsModel !*! ph+    obsGain = ph !*! matInvert innovCov+    errorCov' = kalmanCovariance prior !-! obsGain !*! obsModel !*! kalmanCovariance prior+    state' = kalmanState prior ^+^ (obsGain !* innovation)

@@ -0,0 +1,50 @@+{-# OPTIONS_GHC -fno-warn-orphans #-}+{-# LANGUAGE GeneralizedNewtypeDeriving #-}++{- |+Description: Matrix utilities++These functions extend the facilities provided by the 'Linear' module.+They should not be used by external code, but might be useful+contributions to the linear package.+-}++module Numeric.Estimator.Matrix (matInvert) where++import Control.Applicative+import Data.Foldable+import Data.Traversable+import Linear+import Prelude hiding (foldr)++instance Metric []++msplit :: [a] -> [[a]] -> (a, [a], [a], [[a]])+msplit row rows = (first, top, left, rest)+    where+    first : top = row+    (left, rest) = unzip $ map (\ (x:xs) -> (x, xs)) rows++mjoin :: (a, [a], [a], [[a]]) -> [[a]]+mjoin (first, top, left, rest) = (first : top) : (zipWith (\ l r -> l : r) left rest)++matInvertList :: Fractional a => [[a]] -> [[a]]+matInvertList [] = []+matInvertList [[a]] = [[recip a]]+matInvertList (row : rows) = mjoin (a', b', c', d')+    where+    (a, b, c, d) = msplit row rows+    aInv = recip a+    caInv = fmap (* aInv) c+    aInvb = fmap (aInv *) b+    d' = matInvertList $ d !-! outer c aInvb+    c' = negated $ d' !* caInv+    b' = negated $ aInvb *! d'+    a' = aInv + dot aInvb (d' !* caInv)++copyInto :: Traversable f => f a -> [a] -> f a+copyInto structure contents = snd $ mapAccumL (\ (x:xs) _ -> (xs, x)) contents structure++-- | Compute the matrix inverse of a square matrix.+matInvert :: (Traversable f, Fractional a) => f (f a) -> f (f a)+matInvert m = copyInto m $ liftA2 copyInto (toList m) $ matInvertList $ fmap toList $ toList m

@@ -0,0 +1,78 @@+{-# LANGUAGE GeneralizedNewtypeDeriving #-}++{- |+Description: Types for different coordinate systems++The 'Linear' module provides basic fixed-dimensional vector types such+as 'V3', for three-element vectors. However, it does not help with+identifying which coordinate system a vector was measured in.++The types in this module are trivial newtype wrappers around 'V3' to tag+vectors with an appropriate coordinate system. The systems used here+follow a common convention used in navigation problems.+-}++module Numeric.Estimator.Model.Coordinate where++import Control.Applicative+import Data.Distributive+import Data.Foldable+import Data.Traversable+import Linear++-- * Navigation frame++{- |+Navigation occurs in a right-hand coordinate system with respect to a+\"local tangent plane\". The origin of this plane is chosen to be some+convenient point on the Earth's surface--perhaps the location where+navigation began. The plane is oriented such that it is tangent to the+Earth's surface at that origin point. The basis vectors point northward,+eastward, and downward from the origin. Notice that the further you+travel from the origin, the further the tangent plane separates from the+surface of the Earth, so this approach is of limited use over long+distances.+-}+newtype NED a = NED { nedToVec3 :: V3 a }+    deriving (Show, Additive, Applicative, Distributive, Foldable, Functor, Metric, Num, Traversable)++-- | Construct a navigation frame coordinate from (north, east, down).+ned :: a -> a -> a -> NED a+ned n e d = NED $ V3 n e d++-- * Body frame++{- |+Most sensor measurements are taken with respect to the sensor+platform in the vehicle. We assume the sensors are perfectly+orthogonally arranged in a right-hand Cartesian coordinate system, which+is usually close enough to the truth, although more sophisticated+approaches exist to calibrate out non-orthogonal alignment and other+errors. This coordinate system is only meaningful with respect to the+current position and orientation of the sensor platform, as of the+instant that the measurement was taken.+-}+newtype XYZ a = XYZ { xyzToVec3 :: V3 a }+    deriving (Show, Additive, Applicative, Distributive, Foldable, Functor, Metric, Num, Traversable)++-- | Construct a body frame coordinate from (x, y, z).+xyz :: a -> a -> a -> XYZ a+xyz a b c = XYZ $ V3 a b c++-- * Coordinate frame conversion++{- |+Most practical problems involving inertial sensors (such as+accelerometers and gyroscopes) require keeping track of the relationship+between these two coordinate systems.++If you maintain a quaternion representing the rotation from navigation+frame to body frame, then you can use this function to get functions+that will convert coordinates between frames in either direction.+-}+convertFrames :: Num a => Quaternion a -> (XYZ a -> NED a, NED a -> XYZ a)+convertFrames q = (toNav, toBody)+    where+    rotate2nav = NED $ fmap XYZ $ fromQuaternion q+    toNav = (rotate2nav !*)+    toBody = (transpose rotate2nav !*)

@@ -0,0 +1,26 @@+{- |+Description: Simplified atmosphere model++This module implements the+<http://en.wikipedia.org/wiki/U.S._Standard_Atmosphere#1976_version 1976 U.S. Standard Atmosphere>,+but is only valid for altitudes from sea level to 11km.+-}++module Numeric.Estimator.Model.Pressure (pressureToHeight, heightToPressure) where++basePressure, baseTemperature, lapseRate, baseAltitude, airGasConstant, g_0, airMass :: Floating a => a+basePressure = 101325 -- Pascals+baseTemperature = 288.15 -- K+lapseRate = -0.0065 -- K/m+baseAltitude = 0 -- m+airGasConstant = 8.31432 --  N-m/mol-K+g_0 = 9.80665 -- m/s/s+airMass = 0.0289644 -- kg/mol++-- | Given a barometric pressure measurement in Pascals, return altitude in meters.+pressureToHeight :: Floating a => a -> a+pressureToHeight pressure = (baseAltitude * lapseRate + baseTemperature / ((pressure / basePressure) ** (airGasConstant * lapseRate / (g_0 * airMass))) - baseTemperature) / lapseRate++-- | Given altitude in meters, return a barometric pressure measurement in Pascals.+heightToPressure :: Floating a => a -> a+heightToPressure height = basePressure * (baseTemperature / (baseTemperature + lapseRate * (height - baseAltitude))) ** (g_0 * airMass / (airGasConstant * lapseRate))

@@ -0,0 +1,302 @@+{- |+Description: Sample estimator model for sensor fusion++Many kinds of vehicles have a collection of sensors for measuring where+they are and where they're going, which may include these sensors and+others:++- accelerometers++- gyroscopes++- GPS receiver++- pressure altimeter++- 3D magnetometer++Each of these sensors provides some useful information about the current+physical state of the vehicle, but they all have two obnoxious problems:++1. No one sensor provides all the information you want at the update+rate you need. GPS gives you absolute position, but at best only ten+times per second. Accelerometers can report measurements at high speeds,+hundreds to thousands of times per second, but to get position you have+to double-integrate the measurement samples.++2. Every sensor is lying to you. They measure some aspect of the+physical state, plus some random error. If you have to integrate these+measurements, as with acceleration for instance, then the error+accumulates over time. If you take the derivative, perhaps because you+have position but you need velocity, the derivative amplifies the noise.++This is an ideal case for a state-space estimation algorithm. Once+you've specified the kinetic model of the physical system, and modeled+each of your sensors, and identified the noise parameters for+everything, the estimation algorithm is responsible for combining all+the measurements. The estimator will decide how much to trust each+sensor based on how much confidence it has in its current state+estimate, and how well that state agrees with the current measurement.++This module implements a system model for sensor fusion. With+appropriate noise parameters, it should work for a wide variety of+vehicle types and sensor platforms, whether on land, sea, air, or space.+However, it has been implemented specifically for quad-copter+autopilots. As a result the state vector may have components your system+does not need, or be missing ones you do need.+-}++module Numeric.Estimator.Model.SensorFusion where++import Control.Applicative+import Control.Lens+import Data.Distributive+import Data.Foldable+import Data.Traversable+import Linear+import Numeric.Estimator.Augment+import Numeric.Estimator.Quaternion+import Numeric.Estimator.Model.Coordinate+import Numeric.Estimator.Model.Pressure+import Numeric.Estimator.Model.Symbolic+import Prelude hiding (foldl1)++-- | A collection of all the state variables needed for this model.+data StateVector a = StateVector+    { stateOrient :: !(Quaternion a) -- ^ quaternions defining attitude of body axes relative to local NED+    , stateVel :: !(NED a) -- ^ NED velocity - m/sec+    , statePos :: !(NED a) -- ^ NED position - m+    , stateGyroBias :: !(XYZ a) -- ^ delta angle bias - rad+    , stateWind :: !(NED a) -- ^ NED wind velocity - m/sec+    , stateMagNED :: !(NED a) -- ^ NED earth fixed magnetic field components - milligauss+    , stateMagXYZ :: !(XYZ a) -- ^ XYZ body fixed magnetic field measurements - milligauss+    }+    deriving Show++instance Additive StateVector where+    zero = pure 0++instance Applicative StateVector where+    pure v = StateVector+        { stateOrient = pure v+        , stateVel = pure v+        , statePos = pure v+        , stateGyroBias = pure v+        , stateWind = pure v+        , stateMagNED = pure v+        , stateMagXYZ = pure v+        }+    v1 <*> v2 = StateVector+        { stateOrient = stateOrient v1 <*> stateOrient v2+        , stateVel = stateVel v1 <*> stateVel v2+        , statePos = statePos v1 <*> statePos v2+        , stateGyroBias = stateGyroBias v1 <*> stateGyroBias v2+        , stateWind = stateWind v1 <*> stateWind v2+        , stateMagNED = stateMagNED v1 <*> stateMagNED v2+        , stateMagXYZ = stateMagXYZ v1 <*> stateMagXYZ v2+        }++instance Functor StateVector where+    fmap = liftA++instance Foldable StateVector where+    foldMap = foldMapDefault++instance Traversable StateVector where+    sequenceA v = StateVector+        <$> sequenceA (stateOrient v)+        <*> sequenceA (stateVel v)+        <*> sequenceA (statePos v)+        <*> sequenceA (stateGyroBias v)+        <*> sequenceA (stateWind v)+        <*> sequenceA (stateMagNED v)+        <*> sequenceA (stateMagXYZ v)++instance Distributive StateVector where+    distribute f = StateVector+        { stateOrient = distribute $ fmap stateOrient f+        , stateVel = distribute $ fmap stateVel f+        , statePos = distribute $ fmap statePos f+        , stateGyroBias = distribute $ fmap stateGyroBias f+        , stateWind = distribute $ fmap stateWind f+        , stateMagNED = distribute $ fmap stateMagNED f+        , stateMagXYZ = distribute $ fmap stateMagXYZ f+        }++-- | Define the control (disturbance) vector. Error growth in the inertial+-- solution is assumed to be driven by 'noise' in the delta angles and+-- velocities, after bias effects have been removed. This is OK becasue we+-- have sensor bias accounted for in the state equations.+data DisturbanceVector a = DisturbanceVector+    { disturbanceGyro :: !(XYZ a) -- ^ XYZ body rotation rate in rad/second+    , disturbanceAccel :: !(XYZ a) -- ^ XYZ body acceleration in meters\/second\/second+    }+    deriving Show++instance Applicative DisturbanceVector where+    pure v = DisturbanceVector+        { disturbanceGyro = pure v+        , disturbanceAccel = pure v+        }+    v1 <*> v2 = DisturbanceVector+        { disturbanceGyro = disturbanceGyro v1 <*> disturbanceGyro v2+        , disturbanceAccel = disturbanceAccel v1 <*> disturbanceAccel v2+        }++instance Functor DisturbanceVector where+    fmap = liftA++instance Foldable DisturbanceVector where+    foldMap = foldMapDefault++instance Traversable DisturbanceVector where+    sequenceA v = DisturbanceVector+        <$> sequenceA (disturbanceGyro v)+        <*> sequenceA (disturbanceAccel v)++instance Distributive DisturbanceVector where+    distribute f = DisturbanceVector+        { disturbanceGyro = distribute $ fmap disturbanceGyro f+        , disturbanceAccel = distribute $ fmap disturbanceAccel f+        }++-- * Model initialization++-- | Initial covariance for this model.+initCovariance :: Fractional a => StateVector (StateVector a)+initCovariance = kronecker $ fmap (^ (2 :: Int)) $ StateVector+    { stateOrient = pure 0.1+    , stateVel = pure 0.7+    , statePos = ned 15 15 5+    , stateGyroBias = pure $ 1 * deg2rad+    , stateWind = pure 8+    , stateMagNED = pure 0.02+    , stateMagXYZ = pure 0.02+    }+    where+    deg2rad = realToFrac (pi :: Double) / 180++-- | When the sensor platform is not moving, a three-axis accelerometer+-- will sense an approximately 1g acceleration in the direction of+-- gravity, which gives us the platform's orientation aside from not+-- knowing the current rotation around the gravity vector.+--+-- At the same time, a 3D magnetometer will sense the platform's+-- orientation with respect to the local magnetic field, aside from not+-- knowing the current rotation around the magnetic field line.+--+-- Putting these two together gives the platform's complete orientation+-- since the gravity vector and magnetic field line aren't collinear.+initAttitude :: (Floating a, HasAtan2 a)+             => XYZ a+             -- ^ initial accelerometer reading+             -> XYZ a+             -- ^ initial magnetometer reading+             -> a+             -- ^ local magnetic declination from true North+             -> Quaternion a+             -- ^ computed initial attitude+initAttitude (XYZ accel) (XYZ mag) declination = foldl1 quatMul $ map (uncurry rotateAround)+    [ (ez, initialHdg)+    , (ey, initialPitch)+    , (ex, initialRoll)+    ]+    where+    initialRoll = arctan2 (negate (accel ^._y)) (negate (accel ^._z))+    initialPitch = arctan2 (accel ^._x) (negate (accel ^._z))+    magX = (mag ^._x) * cos initialPitch + (mag ^._y) * sin initialRoll * sin initialPitch + (mag ^._z) * cos initialRoll * sin initialPitch+    magY = (mag ^._y) * cos initialRoll - (mag ^._z) * sin initialRoll+    initialHdg = arctan2 (negate magY) magX + declination+    rotateAround axis theta = Quaternion (cos half) $ pure 0 & el axis .~ (sin half) where half = theta / 2++-- | Compute an initial filter state from an assortment of initial+-- measurements.+initDynamic :: (Floating a, HasAtan2 a)+            => XYZ a+             -- ^ initial accelerometer reading+            -> XYZ a+             -- ^ initial magnetometer reading+            -> XYZ a+             -- ^ initial magnetometer bias+            -> a+             -- ^ local magnetic declination from true North+            -> NED a+             -- ^ initial velocity+            -> NED a+             -- ^ initial position+            -> StateVector a+             -- ^ computed initial state+initDynamic accel mag magBias declination vel pos = (pure 0)+    { stateOrient = initQuat+    , stateVel = vel+    , statePos = pos+    , stateMagNED = initMagNED+    , stateMagXYZ = magBias+    }+    where+    initMagXYZ = mag - magBias+    initQuat = initAttitude accel initMagXYZ declination+    initMagNED = fst (convertFrames initQuat) initMagXYZ+    -- TODO: re-implement InertialNav's calcEarthRateNED++-- * Model equations++-- | This is the kinematic sensor fusion process model, driven by+-- accelerometer and gyro measurements.+processModel :: Fractional a+             => a+             -- ^ time since last process model update+             -> AugmentState StateVector DisturbanceVector a+             -- ^ prior (augmented) state+             -> AugmentState StateVector DisturbanceVector a+             -- ^ posterior (augmented) state+processModel dt (AugmentState state dist) = AugmentState state' $ pure 0+    where+    state' = state+        -- Discretization of @qdot = 0.5 * <0, deltaAngle> * q@.+        --+        --  * /Strapdown Inertial Navigation Technology, 2nd Ed/, section 11.2.5 (on+        --    pages 319-320) gives qdot and its analytic discretization, without proof.+        --  * http://en.wikipedia.org/wiki/Discretization derives the general form of+        --    discretization.+        --  * http://www.euclideanspace.com/physics/kinematics/angularvelocity/QuaternionDifferentiation2.pdf+        --    derives qdot from angular momentum.+        { stateOrient = stateOrient state `quatMul` deltaQuat+        , stateVel = stateVel state + deltaVel+        , statePos = statePos state + fmap (* dt) (stateVel state + fmap (/ 2) deltaVel)+        -- remaining state vector elements are unchanged by the process model+        }+    -- Even fairly low-order approximations introduce error small enough+    -- that it's swamped by other filter errors.+    deltaQuat = approxAxisAngle 3 $ xyzToVec3 $ fmap (* dt) $ disturbanceGyro dist - stateGyroBias state+    deltaVel = fmap (* dt) $ body2nav state (disturbanceAccel dist) + g+    g = ned 0 0 9.80665 -- NED gravity vector - m/sec^2++-- | Compute the local air pressure from the state vector. Useful as a+-- measurement model for a pressure sensor.+statePressure :: Floating a => StateVector a -> a+statePressure = heightToPressure . negate . (^._z) . nedToVec3 . statePos++-- | Compute the true air-speed of the sensor platform. Useful as a+-- measurement model for a true air-speed sensor.+stateTAS :: Floating a => StateVector a -> a+stateTAS state = distance (stateVel state) (stateWind state)++-- | Compute the expected body-frame magnetic field strength and+-- direction, given the hard-iron correction and local+-- declination-adjusted field from the state vector. Useful as a+-- measurement model for a 3D magnetometer.+stateMag :: Num a => StateVector a -> XYZ a+stateMag state = stateMagXYZ state + nav2body state (stateMagNED state)++-- * Helpers++-- | Convert body-frame to navigation-frame given the orientation from+-- this state vector.+body2nav :: Num a => StateVector a -> XYZ a -> NED a+body2nav = fst . convertFrames . stateOrient++-- | Convert navigation-frame to body-frame given the orientation from+-- this state vector.+nav2body :: Num a => StateVector a -> NED a -> XYZ a+nav2body = snd . convertFrames . stateOrient

@@ -0,0 +1,30 @@+{-# OPTIONS_GHC -fno-warn-orphans #-}++{- |+Description: Support for purely symbolic models++This package supports running filters in pure Haskell, of course. But+it's also designed to work with libraries like+<http://hackage.haskell.org/package/sbv sbv> and+<http://hackage.haskell.org/package/ivory ivory> that can extract+symbolic expressions, whether for analysis in other tools or for+generating code in some other language.++This module provides helpers allowing models to abstract away from+standard Haskell type-classes that do not support symbolic computation.+-}++module Numeric.Estimator.Model.Symbolic where++-- | The 'atan2' function is defined in the 'RealFloat' typeclass, which+-- can't be implemented for symbolic types because nearly every member+-- besides 'atan2' returns concrete values, not symbolic ones. This+-- typeclass describes types, symbolic or concrete, that support an+-- 'atan2' function.+class HasAtan2 a where+    -- | Another name for 'atan2', chosen not to collide with+    -- 'RealFloat'.+    arctan2 :: a -> a -> a++instance HasAtan2 Double where+    arctan2 = atan2

@@ -0,0 +1,41 @@+{- |+Description: Quaternion utilities++These functions extend the facilities provided by the 'Linear' module.+They should not be used by external code, but might be useful+contributions to the linear package.+-}++module Numeric.Estimator.Quaternion where++import Data.Foldable+import Linear+import Prelude hiding (foldr, sum)++{- |+The Taylor series expansion of the quaternion axis-angle formula never+divides by any quantity that might be zero. It also avoids computing+fancy floating-point functions like sin, cos, or sqrt. And since non-x86+CPUs typically don't have those fancy functions in hardware, on those+platforms this implementation is as efficient as we're going to get and+allows us to control the tradeoff between computation time and accuracy+of the result.+-}+approxAxisAngle :: Fractional a => Int -> V3 a -> Quaternion a+approxAxisAngle order rotation = Quaternion c $ fmap (s *) rotation+    where+    halfSigmaSq = 0.25 * sum (fmap (^ (2 :: Int)) rotation)+    go prev idx = let cosTerm = prev / fromIntegral (negate idx); sinTerm = cosTerm / fromIntegral (idx + 1) in cosTerm : sinTerm : go (sinTerm * halfSigmaSq) (idx + 2)+    combine term (l, r) = (r + term, l)+    (c, s2) = foldr combine (1, 1) $ take (order - 1) $ go halfSigmaSq (2 :: Int)+    s = 0.5 * s2++{- |+Linear's 'Num' instance for 'Quaternion' requires 'RealFloat' in order+to implement 'signum', but we don't want to require 'RealFloat' as it+doesn't work for symbolic types. This is a copy of just the '*'+implementation, which only needed 'Num'.+-}+quatMul :: Num a => Quaternion a -> Quaternion a -> Quaternion a+quatMul (Quaternion s1 v1) (Quaternion s2 v2)+    = Quaternion (s1 * s2 - (v1 `dot` v2)) $ (v1 `cross` v2) + s1 *^ v2 + s2 *^ v1