ahrs

package
v0.0.0-...-c0701d1 Latest Latest
Warning

This package is not in the latest version of its module.

 Go to latest Published: Aug 13, 2019 License: MIT Imports: 6 Imported by: 0

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

View Source 
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

View Source 
var Kalman0JSONConfig = `` /* 256-byte string literal not displayed */
View Source 
var Kalman1JSONConfig = `` /* 576-byte string literal not displayed */
View Source 
var KalmanJSONConfig = ""
View Source 
var SimpleJSONConfig = `` /* 1356-byte string literal not displayed */

Functions

func AngleDiff 

func AngleDiff(a, b float64) (diff float64)

func FromQuaternion 

func FromQuaternion(q0, q1, q2, q3 float64) (phi float64, theta float64, psi float64)

FromQuaternion calculates the Tait-Bryan angles phi, theta, psi corresponding to the quaternion

func MakeHardSoftRotationMatrix 

func MakeHardSoftRotationMatrix(h1, s1, h2, s2 [3]float64) (rotmat *[3][3]float64, err error)

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 

func MakeOrthogonal(target, ortho [3]float64) (res *[3]float64)

MakeOrthogonal returns a vector close to the input target vector but with the projection on the input ortho vector removed.

func MakePerpendicular 

func MakePerpendicular(vec1, vec2 [3]float64) (res *[3]float64, err error)

MakePerpendicular returns a vector that is perpendicular to both input vectors. (Uses the cross product.)

func MakeUnitVector 

func MakeUnitVector(vec [3]float64) (res *[3]float64, err error)

MakeUnitVector re-scales the vector vec into a unit vector.

func NewVarianceAccumulator 

func NewVarianceAccumulator(m0, v0, decay float64) func(float64) (float64, float64, float64)

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 

func QuaternionAToB(a1, a2, a3, b1, b2, b3 float64) (q0, q1, q2, q3 float64)

QuaternionAToB constructs a quaternion Q such that QAQ* = B for arbitrary vectors A and B

func QuaternionNormalize 

func QuaternionNormalize(q0, q1, q2, q3 float64) (r0, r1, r2, r3 float64)

QuaternionNormalize re-scales the input quaternion to unit norm.

func QuaternionRotate 

func QuaternionRotate(q0, q1, q2, q3, h1, h2, h3 float64) (r0, r1, r2, r3 float64)

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 

func QuaternionSign(q0, q1, q2, q3, r0, r1, r2, r3 float64) (s0, s1, s2, s3 float64)

QuaternionSign chooses the sign of quaternion q so that it is minimally distant from quaternion r.

func QuaternionToRotationMatrix 

func QuaternionToRotationMatrix(q0, q1, q2, q3 float64) (r *[3][3]float64)

QuaternionToRotationMatrix computes the rotation matrix r corresponding to a quaternion q.

func Regularize 

func Regularize(roll, pitch, heading float64) (float64, float64, float64)

Regularize ensures that roll, pitch, and heading are in the correct ranges. All in radians.

func RotationMatrixToQuaternion 

func RotationMatrixToQuaternion(r [3][3]float64) (q0, q1, q2, q3 float64)

RotationMatrixToQuaternion computes the quaternion q corresponding to a rotation matrix r.

func ToQuaternion 

func ToQuaternion(phi, theta, psi float64) (float64, float64, float64, float64)

ToQuaternion calculates the 0,1,2,3 components of the rotation quaternion corresponding to the Tait-Bryan angles phi, theta, psi

func VarFromQuaternion 

func VarFromQuaternion(q0, q1, q2, q3, dq0, dq1, dq2, dq3 float64) (float64, float64, float64)

VarFromQuaternion returns the standard deviation of the Tate-Bryan angles phi, theta, psi corresponding to the quaternion q0, q1, q2, q3 with stdev dq0, dq1, dq2, dq3

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 and gyro calibrations to d.
	SetCalibrations(c, d *[3]float64)
	// GetCalibrations returns the AHRS accelerometer calibrations c and gyro calibrations d.
	GetCalibrations() (c, d *[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
	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 

func (s *State) CalcRollPitchHeading() (roll float64, pitch float64, heading float64)

RollPitchHeading returns the current roll, pitch and heading estimates for the State, in degrees

func (*State) GLoad 

func (s *State) GLoad() (gLoad float64)

GLoad returns the current G load, in G's.

func (*State) GetCalibrations 

func (s *State) GetCalibrations() (c, d *[3]float64)

GetCalibrations sets the AHRS accelerometer calibrations to c and gyro calibrations to d.

func (*State) GetLogMap 

func (s *State) GetLogMap() (p map[string]interface{})

GetLogMap returns a map providing current state and measurement values for analysis

func (*State) GetSensorQuaternion 

func (s *State) GetSensorQuaternion() *[4]float64

GetSensorQuaternion returns the AHRS algorithm's sensor quaternion F.

func (*State) GetState 

func (s *State) GetState() *State

GetState returns the state of the system

func (*State) MagHeading 

func (s *State) MagHeading() (hdg float64)

MagHeading returns the magnetic heading in degrees.

func (*State) RateOfTurn 

func (s *State) RateOfTurn() (turnRate float64)

RateOfTurn returns the turn rate in degrees per second.

func (*State) Reset 

func (s *State) Reset()

Reset restarts the algorithm from scratch.

func (*State) RollPitchHeading 

func (s *State) RollPitchHeading() (roll float64, pitch float64, heading float64)

RollPitchHeading returns the current attitude values as estimated by the Kalman algorithm.

func (*State) RollPitchHeadingUncertainty 

func (s *State) RollPitchHeadingUncertainty() (droll float64, dpitch float64, dheading float64)

func (*State) SetCalibrations 

func (s *State) SetCalibrations(c, d *[3]float64)

SetCalibrations sets the AHRS accelerometer calibrations to c and gyro calibrations to d.

func (*State) SetConfig 

func (s *State) SetConfig(configMap map[string]float64)

SetConfig lets the user alter some of the configuration settings.

func (*State) SetSensorQuaternion 

func (s *State) SetSensorQuaternion(f *[4]float64)

SetSensorQuaternion changes the AHRS algorithm's sensor quaternion F.

func (*State) SlipSkid 

func (s *State) SlipSkid() (slipSkid float64)

SlipSkid returns the slip/skid angle in degrees.

func (*State) Valid 

func (s *State) Valid() (ok bool)

Valid returns whether the current state is a valid estimate or if something went wrong in the calculation.

Source Files

View all Source files

Jump to

Keyboard shortcuts

? : This menu
/ : Search site
f or F : Jump to
y or Y : Canonical URL
go.dev uses cookies from Google to deliver and enhance the quality of its services and to analyze traffic. Learn more.