| Safe Haskell | None |
|---|---|
| Language | Haskell2010 |
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
- 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 double-integrate 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 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.
- 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.
Constructors
| StateVector | |
Fields
| |
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.
Constructors
| DisturbanceVector | |
Fields
| |
Model initialization
initCovariance :: Fractional a => StateVector (StateVector a) Source
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.