EKF Algorithms

This section develops the equations that form the basis of an Extended Kalman Filter (EKF), which calculates position, velocity, and orientation of a body in space[1]. In a VG, AHRS, or INS[2] application, inertial sensor readings are used to form high data-rate (DR) estimates of the system states while less frequent or noisier measurements (GPS and inertial sensors) act as references to correct errors in the system.

In addition to deriving the EKF equations, this description presents a measurement model based on Euler angles, which result from accelerometers, magnetometers, and GPS readings. Following that it describes implementations that result in improved solutions under both static and dynamic conditions. Finally, a series of examples illustrate existing VG, AHRS, and INS algorithms.

The algorithm development description is broken up into a series of sections that build upon one another, as follows:

  • Coordinate Frames
  • Attitude Parameters
  • Sensors
  • Extended Kalman Filter
  • Process Models
  • Measurement Model
  • Measurement Vector
  • Innovation (Measurement Error)
  • Magnetic Alignment
  • References
[1]This discussion presupposes a certain amount of knowledge. Details related to differential equations, linear algebra, multi-variable calculus, stochastic processes, etc. are not described.
[2]A VG uses rate-sensors and accelerometers to estimate roll and pitch. An AHRS incorporates magnetometer readings to the VG to estimate heading. An INS adds GPS messages to the VG or AHRS to estimate position and velocity or provide a way to estimate heading without magnetometers.