State Transition Models¶
System State-Transition Model Summary[1]¶
The state-transition models form the core of the EKF prediction stage by performing the following roles:
- They form the equations that propagate the system states from one time-step to the next (using high-quality sensor as the input)
- They define the process-noise vectors relating each state to sensor noise
- They enable computation of the process covariance matrix, Q, and process Jacobian, F. Both are used to propagate the system covariance, P, from one time-step to the next.
The complete system state equation consists of 16 total states[2]
with the state-transition model, \(\vec{f}\), made up of five individual models (developed in upcoming sections):
where \(\vec{x}\) is the state-vector, \(\vec{u}\) is the input-vector (consisting of sensor signals) and \(\vec{w}\) is the process-noise vector.
The expanded state-transition vector, \(\vec{f}\), is:
and the process-noise vector, \(\vec{w}_{k-1}\), is:
The sensor noise vectors, \(\vec{N}\), corresponding to the angular-rate and accelerometer bias states, are each 3x1 vectors with elements described by a zero-mean Gaussian distribution with a variance of either \(\sigma_{dd,\omega}^{2}\) or \(\sigma_{dd,a}^{2}\).
Individual State-Transition Models¶
Individual state-transition models are derived in the following sections:
[1] | There are many papers describing the derivation and implementation issues for EKFs and Complementary-Filters. Several of the papers similar to this implementation are referenced in the Reference section. |
[2] | GPS measurements are in latitude/longitude/altitude. These are converted to position in the Earth-frame, \(\vec{r}{^E}\). Position in the NED-frame is calculated from the initial starting point at system startup. The state estimate is generated by integrating velocity (estimated from accelerometer data). |