Documentation ¶
Overview ¶
Package ahrs implements a Kalman filter for determining aircraft kinematic state based on inputs from IMU and GPS
The idea behind the Simple AHRS algorithm is to use the GPS to compute what the accelerometer should show and then to create a rotation matrix to map the measured accelerometer vector onto this vector and the speed vector (GPS Track) onto the sensor x-axis. The gyro is used to further improve this estimate, by providing a more instantaneous response than the GPS can provide. This is really a poor-man's sensor fusion algorithm. The proper way to do this is with a Kalman Filter, but this approach is simpler and easier to debug, and should be good enough for most flight conditions.
It is a step on the way to the full Kalman Filter implementation, a bit more obvious what's going on so the math and stratux integration can be more easily developed and debugged.
This software is protected by the MIT license and is copyright 2017 by Eric Westphal. I would be happy for the algorithm to be used elsewhere but please do give me credit if you use either this specific software or the algorithm behind it.
Index ¶
- Constants
- Variables
- func AngleDiff(a, b float64) (diff float64)
- func FromQuaternion(q0, q1, q2, q3 float64) (phi float64, theta float64, psi float64)
- func MakeHardSoftRotationMatrix(h1, s1, h2, s2 [3]float64) (rotmat *[3][3]float64, err error)
- func MakeOrthogonal(target, ortho [3]float64) (res *[3]float64)
- func MakePerpendicular(vec1, vec2 [3]float64) (res *[3]float64, err error)
- func MakeUnitVector(vec [3]float64) (res *[3]float64, err error)
- func NewVarianceAccumulator(m0, v0, decay float64) func(float64) (float64, float64, float64)
- func QuaternionAToB(a1, a2, a3, b1, b2, b3 float64) (q0, q1, q2, q3 float64)
- func QuaternionNormalize(q0, q1, q2, q3 float64) (r0, r1, r2, r3 float64)
- func QuaternionRotate(q0, q1, q2, q3, h1, h2, h3 float64) (r0, r1, r2, r3 float64)
- func QuaternionSign(q0, q1, q2, q3, r0, r1, r2, r3 float64) (s0, s1, s2, s3 float64)
- func QuaternionToRotationMatrix(q0, q1, q2, q3 float64) (r *[3][3]float64)
- func Regularize(roll, pitch, heading float64) (float64, float64, float64)
- func RotationMatrixToQuaternion(r [3][3]float64) (q0, q1, q2, q3 float64)
- func ToQuaternion(phi, theta, psi float64) (float64, float64, float64, float64)
- func VarFromQuaternion(q0, q1, q2, q3, dq0, dq1, dq2, dq3 float64) (float64, float64, float64)
- type AHRSLogger
- type AHRSProvider
- type Kalman0State
- type Kalman1State
- type KalmanState
- func (s *KalmanState) CalcRollPitchHeadingUncertainty() (droll float64, dpitch float64, dheading float64)
- func (s *KalmanState) Compute(m *Measurement)
- func (s *KalmanState) GetState() *State
- func (s *KalmanState) GetStateMap() (dat *map[string]float64)
- func (s *KalmanState) Predict(t float64)
- func (s *KalmanState) PredictMeasurement() (m *Measurement)
- func (s *KalmanState) Update(m *Measurement)
- func (s *KalmanState) Valid() (ok bool)
- type Measurement
- type SimpleState
- type State
- func (s *State) CalcRollPitchHeading() (roll float64, pitch float64, heading float64)
- func (s *State) GLoad() (gLoad float64)
- func (s *State) GetCalibrations() (c, d, k, l *[3]float64)
- func (s *State) GetLogMap() (p map[string]interface{})
- func (s *State) GetSensorQuaternion() *[4]float64
- func (s *State) GetState() *State
- func (s *State) MagHeading() (hdg float64)
- func (s *State) RateOfTurn() (turnRate float64)
- func (s *State) Reset()
- func (s *State) RollPitchHeading() (roll float64, pitch float64, heading float64)
- func (s *State) RollPitchHeadingUncertainty() (droll float64, dpitch float64, dheading float64)
- func (s *State) SetCalibrations(c, d, k, l *[3]float64)
- func (s *State) SetConfig(configMap map[string]float64)
- func (s *State) SetSensorQuaternion(f *[4]float64)
- func (s *State) SlipSkid() (slipSkid float64)
- func (s *State) Valid() (ok bool)
Constants ¶
const ( Pi = math.Pi G = 32.1740 / 1.687810 // G is the acceleration due to gravity, kt/s Small = 1e-9 Big = 1e9 Deg = Pi / 180 MMDecay = 1 - 1.0/50 // Exponential decay constant for measurement variances Invalid float64 = 3276.7 // 2**15-1 )
Variables ¶
var Kalman0JSONConfig = `` /* 256-byte string literal not displayed */
var Kalman1JSONConfig = `` /* 576-byte string literal not displayed */
var KalmanJSONConfig = ""
var SimpleJSONConfig = `` /* 1356-byte string literal not displayed */
Functions ¶
func FromQuaternion ¶
FromQuaternion calculates the Tait-Bryan angles phi, theta, psi corresponding to the quaternion
func MakeHardSoftRotationMatrix ¶
MakeHardSoftRotationMatrix constructs a rotation matrix that rotates the unit vector h1 exactly into the unit vector h2 and the unit vector s1 as nearly as possible into the unit vector s2. h1 is "hard-mapped" into h2 and s1 is "soft-mapped" into s2. It is up to the caller to ensure that all vectors are unit vectors.
func MakeOrthogonal ¶
MakeOrthogonal returns a vector close to the input target vector but with the projection on the input ortho vector removed.
func MakePerpendicular ¶
MakePerpendicular returns a vector that is perpendicular to both input vectors. (Uses the cross product.)
func MakeUnitVector ¶
MakeUnitVector re-scales the vector vec into a unit vector.
func NewVarianceAccumulator ¶
NewVarianceAccumulator(init, decay float64) returns a function that, when passed a float, accumulates the exponentially weighted mean and variance of the first differences with decay constant "decay". The accumulator is initialized with a typical mean m0 and variance v0 and returns the current estimates of the effective number of observations, the mean and the variance.
func QuaternionAToB ¶
QuaternionAToB constructs a quaternion Q such that QAQ* = B for arbitrary vectors A and B
func QuaternionNormalize ¶
QuaternionNormalize re-scales the input quaternion to unit norm.
func QuaternionRotate ¶
QuaternionRotate rotates a quaternion Qea by a small rate-of-change vector Ha (e.g. as measured by a gyro in the aircraft frame), Qea -> Qea + 0.5*Qea*Ha
func QuaternionSign ¶
QuaternionSign chooses the sign of quaternion q so that it is minimally distant from quaternion r.
func QuaternionToRotationMatrix ¶
QuaternionToRotationMatrix computes the rotation matrix r corresponding to a quaternion q.
func Regularize ¶
Regularize ensures that roll, pitch, and heading are in the correct ranges. All in radians.
func RotationMatrixToQuaternion ¶
RotationMatrixToQuaternion computes the quaternion q corresponding to a rotation matrix r.
func ToQuaternion ¶
ToQuaternion calculates the 0,1,2,3 components of the rotation quaternion corresponding to the Tait-Bryan angles phi, theta, psi
Types ¶
type AHRSLogger ¶
type AHRSLogger struct { Header []string // contains filtered or unexported fields }
func NewAHRSLogger ¶
func NewAHRSLogger(filename string, logMap map[string]interface{}) (l *AHRSLogger)
func (*AHRSLogger) Close ¶
func (l *AHRSLogger) Close()
func (*AHRSLogger) Log ¶
func (l *AHRSLogger) Log()
type AHRSProvider ¶
type AHRSProvider interface { // RollPitchHeading returns the current attitude values as estimated by the Kalman algorithm. RollPitchHeading() (roll float64, pitch float64, heading float64) // MagHeading returns the current magnetic heading in degrees as estimated by the Kalman algorithm. MagHeading() (hdg float64) // SlipSkid returns the slip/skid angle in degrees as estimated by the Kalman algorithm. SlipSkid() (slipSkid float64) // RateOfTurn returns the turn rate in degrees per second as estimated by the Kalman algorithm. RateOfTurn() (turnRate float64) // GLoad returns the current G load, in G's as estimated by the Kalman algorithm. GLoad() (gLoad float64) // Compute runs both the "predict" and "update" stages of the algorithm, for convenience. Compute(m *Measurement) // SetSensorQuaternion changes the AHRS algorithm's sensor quaternion F. SetSensorQuaternion(f *[4]float64) // GetSensorQuaternion returns the AHRS algorithm's sensor quaternion F. GetSensorQuaternion() (f *[4]float64) // SetCalibrations sets the AHRS accelerometer calibrations to c, gyro calibrations to d, // mag scaling to k and mag offset to l. SetCalibrations(c, d, k, l *[3]float64) // GetCalibrations returns the AHRS accelerometer calibrations c, gyro calibrations d, // mag scaling k and mag offset l. GetCalibrations() (c, d, k, l *[3]float64) // SetConfig allows for configuration of AHRS to be set on the fly, mainly for developers. SetConfig(configMap map[string]float64) // Valid returns whether the current state is a valid estimate or if something went wrong in the calculation. Valid() bool // Reset restarts the algorithm from scratch. Reset() // GetState returns all the information about the current state. GetState() *State // GetLogMap returns a map customized for each AHRSProvider algorithm to provide more detailed information // for debugging and logging. GetLogMap() map[string]interface{} }
AHRSProvider defines an AHRS (Kalman or other) algorithm, such as ahrs_kalman, ahrs_simple, etc.
type Kalman0State ¶
type Kalman0State struct { State // contains filtered or unexported fields }
func NewKalman0AHRS ¶
func NewKalman0AHRS() (s *Kalman0State)
Initialize the state at the start of the Kalman filter, based on current measurements
func (*Kalman0State) Compute ¶
func (s *Kalman0State) Compute(m *Measurement)
Compute runs first the prediction and then the update phases of the Kalman filter
func (*Kalman0State) SetCalibrations ¶
func (s *Kalman0State) SetCalibrations(c, d *[3]float64)
SetCalibrations sets the AHRS accelerometer calibrations to c and gyro calibrations to d.
type Kalman1State ¶
type Kalman1State struct { State // contains filtered or unexported fields }
func NewKalman1AHRS ¶
func NewKalman1AHRS() (s *Kalman1State)
Initialize the state at the start of the Kalman filter, based on current measurements
func (*Kalman1State) Compute ¶
func (s *Kalman1State) Compute(m *Measurement)
Compute runs first the prediction and then the update phases of the Kalman filter
func (*Kalman1State) SetCalibrations ¶
func (s *Kalman1State) SetCalibrations(c, d *[3]float64)
SetCalibrations sets the AHRS accelerometer calibrations to c and gyro calibrations to d.
type KalmanState ¶
type KalmanState struct {
State
}
func InitializeKalman ¶
func InitializeKalman(m *Measurement) (s *KalmanState)
Initialize the state at the start of the Kalman filter, based on current measurements
func (*KalmanState) CalcRollPitchHeadingUncertainty ¶
func (s *KalmanState) CalcRollPitchHeadingUncertainty() (droll float64, dpitch float64, dheading float64)
func (*KalmanState) Compute ¶
func (s *KalmanState) Compute(m *Measurement)
Compute runs first the prediction and then the update phases of the Kalman filter
func (*KalmanState) GetState ¶
func (s *KalmanState) GetState() *State
GetState returns the Kalman state of the system
func (*KalmanState) GetStateMap ¶
func (s *KalmanState) GetStateMap() (dat *map[string]float64)
GetStateMap returns the state information for analysis
func (*KalmanState) Predict ¶
func (s *KalmanState) Predict(t float64)
Predict performs the prediction phase of the Kalman filter
func (*KalmanState) PredictMeasurement ¶
func (s *KalmanState) PredictMeasurement() (m *Measurement)
func (*KalmanState) Update ¶
func (s *KalmanState) Update(m *Measurement)
Update applies the Kalman filter corrections given the measurements
func (*KalmanState) Valid ¶
func (s *KalmanState) Valid() (ok bool)
Valid applies some heuristics to detect whether the computed state is valid or not
type Measurement ¶
type Measurement struct {
UValid, WValid, SValid, MValid bool // Do we have valid airspeed, GPS, accel/gyro, and magnetometer readings?
// U, W, A, B, M
U1, U2, U3 float64 // Vector of measured airspeed, kt, aircraft (accelerated) frame
W1, W2, W3 float64 // Vector of GPS speed in N/S, E/W and U/D directions, kt, latlong axes, earth (inertial) frame
A1, A2, A3 float64 // Vector holding accelerometer readings, G, aircraft (accelerated) frame
B1, B2, B3 float64 // Vector of gyro rates in roll, pitch, heading axes, °/s, aircraft (accelerated) frame
M1, M2, M3 float64 // Vector of magnetometer readings, µT, aircraft (accelerated) frame
TW, TU, T float64 // Timestamp of GPS, airspeed and sensor readings
Accums [15]func(float64) (float64, float64, float64) // Accumulators to track means & variances of all variables
M *matrix.DenseMatrix // Measurement noise covariance
}
Measurement holds the measurements used for updating the Kalman filter: true airspeed, groundspeed, accelerations, gyro rates, magnetometer, time; along with variance accumulators and uncertainty matrix. Note: some sensors (e.g. airspeed and magnetometer) may not be available until appropriate sensors are working.
func NewMeasurement ¶
func NewMeasurement() (m *Measurement)
NewMeasurement returns a pointer to an empty AHRS Measurement. Uncertainty matrix and variance accumulators are properly initialized.
type SimpleState ¶
type SimpleState struct { State // contains filtered or unexported fields }
func NewSimpleAHRS ¶
func NewSimpleAHRS() (s *SimpleState)
NewSimpleAHRS returns a new Simple AHRS object. It is initialized with a beginning sensor orientation quaternion f0.
func (*SimpleState) Compute ¶
func (s *SimpleState) Compute(m *Measurement)
Compute performs the AHRSSimple AHRS computations.
func (*SimpleState) RateOfTurn ¶
func (s *SimpleState) RateOfTurn() (turnRate float64)
RateOfTurn returns the turn rate in degrees per second.
func (*SimpleState) RollPitchHeading ¶
func (s *SimpleState) RollPitchHeading() (roll float64, pitch float64, heading float64)
RollPitchHeading returns the current attitude values as estimated by the Kalman algorithm.
func (*SimpleState) SetConfig ¶
func (s *SimpleState) SetConfig(configMap map[string]float64)
SetConfig lets the user alter some of the configuration settings.
type State ¶
type State struct {
U1, U2, U3 float64 // Vector for airspeed, aircraft frame, kt
Z1, Z2, Z3 float64 // Vector for rate of change of airspeed, aircraft frame, G
E0, E1, E2, E3 float64 // Quaternion rotating aircraft frame to earth frame
H1, H2, H3 float64 // Vector for gyro rates, earth frame, °/s
N1, N2, N3 float64 // Vector for earth's magnetic field, earth (inertial) frame, µT
V1, V2, V3 float64 // (Bias) Vector for windspeed, earth frame, kt
C1, C2, C3 float64 // Bias vector for accelerometer, sensor frame, G
F0, F1, F2, F3 float64 // (Bias) quaternion rotating aircraft frame to sensor frame
D1, D2, D3 float64 // Bias vector for gyro rates, sensor frame, °/s
K1, K2, K3 float64 // Bias vector for magnetometer direction, sensor frame, µT
L1, L2, L3 float64 // Bias vector for magnetometer direction, sensor frame, µT
T float64 // Time when state last updated
M *matrix.DenseMatrix // Covariance matrix of state uncertainty, same order as above vars:
N *matrix.DenseMatrix // Covariance matrix of state noise per unit time
// contains filtered or unexported fields
}
State holds the complete information describing the state of the aircraft. Aircraft frame is non-inertial: 1 is to nose; 2 is to left wing; 3 is up. Earth frame is inertial: 1 is east; 2 is north; 3 is up. Sensor frame is fixed within aircraft frame, so non-inertial, rotated.
func (*State) CalcRollPitchHeading ¶
RollPitchHeading returns the current roll, pitch and heading estimates for the State, in degrees
func (*State) GetCalibrations ¶
GetCalibrations returns the AHRS accelerometer calibrations c, gyro calibrations d, mag scaling k and mag offset l.
func (*State) GetLogMap ¶
GetLogMap returns a map providing current state and measurement values for analysis
func (*State) GetSensorQuaternion ¶
GetSensorQuaternion returns the AHRS algorithm's sensor quaternion F.
func (*State) MagHeading ¶
MagHeading returns the magnetic heading in degrees.
func (*State) RateOfTurn ¶
RateOfTurn returns the turn rate in degrees per second.
func (*State) RollPitchHeading ¶
RollPitchHeading returns the current attitude values as estimated by the Kalman algorithm.
func (*State) RollPitchHeadingUncertainty ¶
func (*State) SetCalibrations ¶
SetCalibrations sets the AHRS accelerometer calibrations to c, gyro calibrations to d, mag scaling to k and mag offset to l.
func (*State) SetSensorQuaternion ¶
SetSensorQuaternion changes the AHRS algorithm's sensor quaternion F.