Measurement Vector¶
Model Overview¶
It is possible to choose among various measurement models for a given EKF implementation. The particular model is selected based on many factors, one being the limitations of the available measurements. This formulation was selected due to the incomplete knowledge of the magnetic environment of the system and uses the available sensor information as follows:
 Accelerometers “level” the system (used to compute \({^{\perp}}{\phi}{_{meas}^{B}}\) and \({^{\perp}}{\theta}{_{meas}^{B}}\)) FN
 Magnetometers and/or GPS heading information align the perpframe with true or magnetic north (\({^{N}}{\psi}{^{\perp}}\))
 GPS position and velocity measurements update the position and velocity estimates (\(\vec{r}^{N}\) and \(\vec{v}^{N}\))
Based upon these steps, the measurement vector, \(\vec{z}_{k}\), is formed:
with the corresponding measurement model, \(\vec{h}_{k}\):
Both \({^{N}}{\vec{\Theta}}{_{meas}^{B}}\) and \({^{N}}{\vec{\Theta}}{_{pred}^{B}}\) are 3x1 column vectors containing the roll, pitch, and heading values. FN
Measurement Vector (\(\vec{z}_{k}\))¶
The measurement vector, \(\vec{z}_{k}\) is comprised of position, velocity, and attitude information as defined above. It is formed from sensor measurements:. However, only the GPS velocity is available directly from measurements; other information must be derived from sensor readings using the relationship described below.
Roll and Pitch Measurements¶
Roll and pitch values are computed from the accelerometer signal. Under static conditions, measurements made by the accelerometer consists solely of gravity and sensor noise. Along the axis pointed in the direction of gravity, the sensor measures 1 [g]. This is due to the proofmass being pulled in the direction of gravity, which is equivalent to a deceleration of 1 [g] in the absence of gravity.
Static roll and pitch values are determined by noting that gravity is constant in the NFrame (perpFrame):
and can be transformed into the body frame through \({^{B}{R}^{\perp}}\):
Using the definition of \({^{\perp}{R}^{B}}\) (discussed in Attitude Parameters) and expanding the equation, the accelerometer measurements can be related to roll and pitch angles:
From this result, the angles corresponding to the accelerometer signal are found:
where, \(\hat{a}_{mx}^{B}\) is the xaxis acceleration value normalized by the total acceleration magnitude:
Normalization of the y and zaxis accelerometer values can be performed. However this is not required as the \(atan\) function uses the ratio of the two (the normalization factor cancels out).
Heading Measurements¶
Heading measurements are determined from the following:
 Magnetometers
 GPS Velocity
MagnetometerBased Heading¶
Magnetometers measure the local magnetic field at a high DRs but the readings can be affected by hard and softiron disturbances in the system or by changes in the external magnetic field. Hard and softiron effects are local to the system and can be accounted for; external field disturbances cannot be corrected.
Adjustment of the magnetic field measurement for hard/softiron disturbances can be performed according to the following equation:
where \(\vec{m}_{meas}^{B}\) is the measured magnetic field vector in the bodyframe, \(\vec{m}_{HI}^{B}\) is the hardiron disturbance, and \(R_{SI}\) and \(S_{SI}\) are the softiron disturbances. Note: for this analysis the magnetometer bias is neglected; assumed to be negligible or lumped in with the hardiron.
Hard and softiron parameters are estimated by performing a magneticalignment maneuver. Note that the application of these corrections do not adjust individual magnetometer channels to match the actual field strength. Only the relative magnetic field is corrected, resulting in a unitcircle for the xy magneticfield. However, as shown later, this enables the heading to be calculated from the corrected signal.
Heading calculation¶
The heading is computed using the fact that, in the magnetic NEDframe, the yaxis component of the magnetic field is zero. In the truenorth NEDframe this is not the case; a magnetic declination angle corrects for this. The magnetic field at a given point can be found using the World Magnetic Model (WMM) or from NOAA’s website (https://www.ngdc.noaa.gov/geomagweb/#igrfwmm). In San Jose, CA, the magnetic field estimates are provided in Table 4:
Figure 4 illustrates the relationship between the Lines of constant Lat/Lon, the NEDframe, and the perpframe. Declination is specified with \(\delta\) and heading is specified with \(\psi\).
The magnetic field vector, \(\vec{b}\), can be broken down into two components:
 the xyplane component and
 the vertical component
The relationship between heading and magnetic field is based on the components of \(\vec{b}^{N}\) as measured in the NEDframe:
Expanding the expression results in the following:
From this, the heading is computed:
Note: the values for \(b_{x}^{\perp}\) and \(b_{y}^{\perp}\) are the corrected and ‘leveled’ values of the measured magneticfield in the bodyframe; roll and pitch estimates are used to level the signal via \({^{\perp}{R}_{pred}^{B}}\).
Note: as this calculation only corrects the magneticfield in the xy bodyframe, the heading solution is best when the system is nearly level. he solution begins to degrade as the roll and pitch increase. This can be accounted for by adjusting the measurement covariance matrix, \(R\), accordingly. Additionally, the solution also begins to degrade as the iron in the system increases.
GPS Heading¶
Heading is also provided directly from the GPS messages. The four messages currently decoded by the IMU381/OpenIMU firmware provide true heading via messages listed in Table 6.
System  Message  Description  Units 

NovAtel  BESTVEL  Actual direction of motion over
ground (track over ground) with
respect to True North

[deg] 
NMEA  VTG  True track made good  [deg] 
SiRF  Geodetic Navigation
Data – Message ID 41

Course Over Ground
(COG, True)

[deg x 100] 
ublox  NAVVELNED  Heading of motion 2D  [deg] 
of the PS readings and angles derived from accelerometer readings (equations provided in Measurement Covariance section):
GPS Position and Velocity¶
GPSbased position is derived from the GPS lat/lon/alt message (BestPos, GGA, etc) and converted to NEDposition using the WGS84 model.
GPSbased velocity is obtained from the BestVel, etc message. However, the NMEA message does not provide vertical velocity, derived from or accounted for in other ways. In all cases the N and Evelocity is calculated from heading and ground speed. The relationship is: