diff --git a/LICENSE b/LICENSE new file mode 100644 --- /dev/null +++ b/LICENSE @@ -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. diff --git a/Setup.hs b/Setup.hs new file mode 100644 --- /dev/null +++ b/Setup.hs @@ -0,0 +1,2 @@ +import Distribution.Simple +main = defaultMain diff --git a/estimator.cabal b/estimator.cabal new file mode 100644 --- /dev/null +++ b/estimator.cabal @@ -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 or + . 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 diff --git a/src/Numeric/Estimator.hs b/src/Numeric/Estimator.hs new file mode 100644 --- /dev/null +++ b/src/Numeric/Estimator.hs @@ -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 diff --git a/src/Numeric/Estimator/Augment.hs b/src/Numeric/Estimator/Augment.hs new file mode 100644 --- /dev/null +++ b/src/Numeric/Estimator/Augment.hs @@ -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 diff --git a/src/Numeric/Estimator/Class.hs b/src/Numeric/Estimator/Class.hs new file mode 100644 --- /dev/null +++ b/src/Numeric/Estimator/Class.hs @@ -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 diff --git a/src/Numeric/Estimator/KalmanFilter.hs b/src/Numeric/Estimator/KalmanFilter.hs new file mode 100644 --- /dev/null +++ b/src/Numeric/Estimator/KalmanFilter.hs @@ -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) diff --git a/src/Numeric/Estimator/Matrix.hs b/src/Numeric/Estimator/Matrix.hs new file mode 100644 --- /dev/null +++ b/src/Numeric/Estimator/Matrix.hs @@ -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 diff --git a/src/Numeric/Estimator/Model/Coordinate.hs b/src/Numeric/Estimator/Model/Coordinate.hs new file mode 100644 --- /dev/null +++ b/src/Numeric/Estimator/Model/Coordinate.hs @@ -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 !*) diff --git a/src/Numeric/Estimator/Model/Pressure.hs b/src/Numeric/Estimator/Model/Pressure.hs new file mode 100644 --- /dev/null +++ b/src/Numeric/Estimator/Model/Pressure.hs @@ -0,0 +1,26 @@ +{- | +Description: Simplified atmosphere model + +This module implements the +, +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)) diff --git a/src/Numeric/Estimator/Model/SensorFusion.hs b/src/Numeric/Estimator/Model/SensorFusion.hs new file mode 100644 --- /dev/null +++ b/src/Numeric/Estimator/Model/SensorFusion.hs @@ -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 diff --git a/src/Numeric/Estimator/Model/Symbolic.hs b/src/Numeric/Estimator/Model/Symbolic.hs new file mode 100644 --- /dev/null +++ b/src/Numeric/Estimator/Model/Symbolic.hs @@ -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 + and + 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 diff --git a/src/Numeric/Estimator/Quaternion.hs b/src/Numeric/Estimator/Quaternion.hs new file mode 100644 --- /dev/null +++ b/src/Numeric/Estimator/Quaternion.hs @@ -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