Safe Haskell  None 

Language  Haskell2010 
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:
 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 doubleintegrate the measurement samples.
 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 statespace 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 quadcopter autopilots. As a result the state vector may have components your system does not need, or be missing ones you do need.
 data StateVector a = StateVector {
 stateOrient :: !(Quaternion a)
 stateVel :: !(NED a)
 statePos :: !(NED a)
 stateGyroBias :: !(XYZ a)
 stateWind :: !(NED a)
 stateMagNED :: !(NED a)
 stateMagXYZ :: !(XYZ a)
 data DisturbanceVector a = DisturbanceVector {
 disturbanceGyro :: !(XYZ a)
 disturbanceAccel :: !(XYZ a)
 initCovariance :: Fractional a => StateVector (StateVector a)
 initAttitude :: (Floating a, HasAtan2 a) => XYZ a > XYZ a > a > Quaternion a
 initDynamic :: (Floating a, HasAtan2 a) => XYZ a > XYZ a > XYZ a > a > NED a > NED a > StateVector a
 processModel :: Fractional a => a > AugmentState StateVector DisturbanceVector a > AugmentState StateVector DisturbanceVector a
 statePressure :: Floating a => StateVector a > a
 stateTAS :: Floating a => StateVector a > a
 stateMag :: Num a => StateVector a > XYZ a
 body2nav :: Num a => StateVector a > XYZ a > NED a
 nav2body :: Num a => StateVector a > NED a > XYZ a
Documentation
data StateVector a Source #
A collection of all the state variables needed for this model.
StateVector  

data DisturbanceVector a Source #
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.
DisturbanceVector  

Model initialization
initCovariance :: Fractional a => StateVector (StateVector a) Source #
Initial covariance for this model.
:: (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 threeaxis 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.
:: (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
:: 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 airspeed of the sensor platform. Useful as a measurement model for a true airspeed sensor.
stateMag :: Num a => StateVector a > XYZ a Source #
Compute the expected bodyframe magnetic field strength and direction, given the hardiron correction and local declinationadjusted field from the state vector. Useful as a measurement model for a 3D magnetometer.