estimator-1.2.0.0: State-space estimation algorithms such as Kalman Filters

Numeric.Estimator.Model.SensorFusion

Description

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
• 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.

Synopsis

# Documentation

data StateVector a Source #

A collection of all the state variables needed for this model.

Constructors

 StateVector FieldsstateOrient :: !(Quaternion a)quaternions defining attitude of body axes relative to local NEDstateVel :: !(NED a)NED velocity - m/secstatePos :: !(NED a)NED position - mstateGyroBias :: !(XYZ a)delta angle bias - radstateWind :: !(NED a)NED wind velocity - m/secstateMagNED :: !(NED a)NED earth fixed magnetic field components - milligaussstateMagXYZ :: !(XYZ a)XYZ body fixed magnetic field measurements - milligauss

Instances

 Source # Methodsfmap :: (a -> b) -> StateVector a -> StateVector b #(<$) :: a -> StateVector b -> StateVector a # Source # Methodspure :: a -> StateVector a #(<*>) :: StateVector (a -> b) -> StateVector a -> StateVector b #(*>) :: StateVector a -> StateVector b -> StateVector b #(<*) :: StateVector a -> StateVector b -> StateVector a # Source # Methodsfold :: Monoid m => StateVector m -> m #foldMap :: Monoid m => (a -> m) -> StateVector a -> m #foldr :: (a -> b -> b) -> b -> StateVector a -> b #foldr' :: (a -> b -> b) -> b -> StateVector a -> b #foldl :: (b -> a -> b) -> b -> StateVector a -> b #foldl' :: (b -> a -> b) -> b -> StateVector a -> b #foldr1 :: (a -> a -> a) -> StateVector a -> a #foldl1 :: (a -> a -> a) -> StateVector a -> a #toList :: StateVector a -> [a] #null :: StateVector a -> Bool #length :: StateVector a -> Int #elem :: Eq a => a -> StateVector a -> Bool #maximum :: Ord a => StateVector a -> a #minimum :: Ord a => StateVector a -> a #sum :: Num a => StateVector a -> a #product :: Num a => StateVector a -> a # Source # Methodstraverse :: Applicative f => (a -> f b) -> StateVector a -> f (StateVector b) #sequenceA :: Applicative f => StateVector (f a) -> f (StateVector a) #mapM :: Monad m => (a -> m b) -> StateVector a -> m (StateVector b) #sequence :: Monad m => StateVector (m a) -> m (StateVector a) # Source # Methodsdistribute :: Functor f => f (StateVector a) -> StateVector (f a) #collect :: Functor f => (a -> StateVector b) -> f a -> StateVector (f b) #distributeM :: Monad m => m (StateVector a) -> StateVector (m a) #collectM :: Monad m => (a -> StateVector b) -> m a -> StateVector (m b) # Source # Methodszero :: Num a => StateVector a #(^+^) :: Num a => StateVector a -> StateVector a -> StateVector a #(^-^) :: Num a => StateVector a -> StateVector a -> StateVector a #lerp :: Num a => a -> StateVector a -> StateVector a -> StateVector a #liftU2 :: (a -> a -> a) -> StateVector a -> StateVector a -> StateVector a #liftI2 :: (a -> b -> c) -> StateVector a -> StateVector b -> StateVector c # Show a => Show (StateVector a) Source # MethodsshowsPrec :: Int -> StateVector a -> ShowS #show :: StateVector a -> String #showList :: [StateVector a] -> ShowS # 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. Constructors  DisturbanceVector FieldsdisturbanceGyro :: !(XYZ a)XYZ body rotation rate in rad/seconddisturbanceAccel :: !(XYZ a)XYZ body acceleration in meters/second/second Instances  Source # Methodsfmap :: (a -> b) -> DisturbanceVector a -> DisturbanceVector b #(<$) :: a -> DisturbanceVector b -> DisturbanceVector a # Source # Methodspure :: a -> DisturbanceVector a #(<*>) :: DisturbanceVector (a -> b) -> DisturbanceVector a -> DisturbanceVector b # Source # Methodsfold :: Monoid m => DisturbanceVector m -> m #foldMap :: Monoid m => (a -> m) -> DisturbanceVector a -> m #foldr :: (a -> b -> b) -> b -> DisturbanceVector a -> b #foldr' :: (a -> b -> b) -> b -> DisturbanceVector a -> b #foldl :: (b -> a -> b) -> b -> DisturbanceVector a -> b #foldl' :: (b -> a -> b) -> b -> DisturbanceVector a -> b #foldr1 :: (a -> a -> a) -> DisturbanceVector a -> a #foldl1 :: (a -> a -> a) -> DisturbanceVector a -> a #toList :: DisturbanceVector a -> [a] #elem :: Eq a => a -> DisturbanceVector a -> Bool #maximum :: Ord a => DisturbanceVector a -> a #minimum :: Ord a => DisturbanceVector a -> a #sum :: Num a => DisturbanceVector a -> a #product :: Num a => DisturbanceVector a -> a # Source # Methodstraverse :: Applicative f => (a -> f b) -> DisturbanceVector a -> f (DisturbanceVector b) #sequenceA :: Applicative f => DisturbanceVector (f a) -> f (DisturbanceVector a) #mapM :: Monad m => (a -> m b) -> DisturbanceVector a -> m (DisturbanceVector b) #sequence :: Monad m => DisturbanceVector (m a) -> m (DisturbanceVector a) # Source # Methodsdistribute :: Functor f => f (DisturbanceVector a) -> DisturbanceVector (f a) #collect :: Functor f => (a -> DisturbanceVector b) -> f a -> DisturbanceVector (f b) #distributeM :: Monad m => m (DisturbanceVector a) -> DisturbanceVector (m a) #collectM :: Monad m => (a -> DisturbanceVector b) -> m a -> DisturbanceVector (m b) # Show a => Show (DisturbanceVector a) Source # MethodsshowList :: [DisturbanceVector a] -> ShowS #

# Model initialization

Initial covariance for this model.

Arguments

 :: (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

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.

Arguments

 :: (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

Compute an initial filter state from an assortment of initial measurements.

# Model equations

Arguments

 :: Fractional a => a time since last process model update -> AugmentState StateVector DisturbanceVector a prior (augmented) state -> AugmentState StateVector DisturbanceVector a posterior (augmented) state

This is the kinematic sensor fusion process model, driven by accelerometer and gyro measurements.

statePressure :: Floating a => StateVector a -> a Source #

Compute the local air pressure from the state vector. Useful as a measurement model for a pressure sensor.

stateTAS :: Floating a => StateVector a -> a Source #

Compute the true air-speed of the sensor platform. Useful as a measurement model for a true air-speed sensor.

stateMag :: Num a => StateVector a -> XYZ a Source #

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.

# Helpers

body2nav :: Num a => StateVector a -> XYZ a -> NED a Source #

Convert body-frame to navigation-frame given the orientation from this state vector.

nav2body :: Num a => StateVector a -> NED a -> XYZ a Source #

Convert navigation-frame to body-frame given the orientation from this state vector.