Methodology
Estimated state
Eleven elements. Everything the accelerometer and gyro can actually see, plus an
aerodynamic scaling factor to compensate for error.
11 elements
$$ \hat{\mathbf x} = \begin{bmatrix}
\mathbf v & \mathbf q & \boldsymbol\omega_b & k
\end{bmatrix}^{\mathsf T} \in \mathbb{R}^{11} $$
- $\mathbf v$, velocity (inertial frame). Observable, because drag makes specific force a function of it.
- $\mathbf q$, attitude. Observable through the direction of the aerodynamic specific force.
- $\boldsymbol\omega_b$, body rate. Measured directly by the gyro.
- $k$, a scale on the onboard aerodynamic force model, initialised at 1.
The aerodynamic flight and onboard model are not the same: the filter carries
the discrepancy as a state and lets the accelerometer resolve it.
Dead reckoned outside the filter
Two quantities are propagated rather than estimated. Position, because no amount of filtering recovers an unobservable state; and wheel momentum,
because it can be integrated from the commanded torques.
position from the same increment used in the predict step; wheel speeds from commanded torque
$$ \hat{\mathbf r} \mathrel{+}= \hat{\mathbf v}\,\Delta t + \tfrac12 \mathbf a\,\Delta t^2, \qquad
\boldsymbol\eta \mathrel{+}= \boldsymbol\tau_w \Delta t, \qquad
\hat{\boldsymbol\Omega} = \frac{\boldsymbol\eta}{I_w} - A^{\mathsf T}\hat{\boldsymbol\omega}_b $$
Neither position nor wheel momentum are corrected through the flight. Their error grows with time, which is exactly as expected.
Predict
Translation is sufficiently modeled with a single Euler step on the scaled aero force with gravity. Rotation on the other hand,
is much stiffer, requiring higher fidelity integration (RK4).
acceleration, attitude kinematics, wheel-coupled attitude dynamics
$$ \mathbf a = \begin{bmatrix} 0 & 0 & -g \end{bmatrix}^{\mathsf T} + \frac{k\,\mathbf F_{\text{aero}}(\hat{\mathbf x})}{m}, \qquad
\dot{\mathbf q} = \tfrac12 \Xi(\mathbf q)\,\boldsymbol\omega_b $$
$$ \mathbf L_b = J_{\text{eff}}\,\boldsymbol\omega_b + A\boldsymbol\eta, \qquad
\dot{\boldsymbol\omega}_b = J_{\text{eff}}^{-1}\!\left(-A\boldsymbol\tau_w - \boldsymbol\omega_b \times \mathbf L_b\right) $$
The state-transition Jacobian is analytical everywhere except for aerodynamic forces, which is calculated with
forward difference. Only three of eleven columns need differencing.
closed form; only the aero partials are differenced
$$ A_{\mathbf v,\,\mathbf x} = \frac{k}{m}\frac{\partial \mathbf F_{\text{aero}}}{\partial \mathbf x}, \quad
A_{\mathbf v,\,k} = \frac{\mathbf F_{\text{aero}}}{m}, \quad
A_{\mathbf q,\mathbf q} = \tfrac12 \Omega(\boldsymbol\omega_b), \quad
A_{\mathbf q,\boldsymbol\omega} = \tfrac12 \Xi(\mathbf q) $$
$$ A_{\boldsymbol\omega,\boldsymbol\omega} = J_{\text{eff}}^{-1}\!\left([\mathbf L_b]_\times - [\boldsymbol\omega_b]_\times J_{\text{eff}}\right), \qquad
A_d = I + A\,\Delta t, \qquad
P^- = A_d\, P\, A_d^{\mathsf T} + Q\,\Delta t $$
Commanded torque is clipped to the wheel limit before
it enters the prediction, so the filter propagates what the actuator can actually deliver.
Update
The gyro sees body rate directly. The accelerometer sees the scaled aerodynamic specific force
in body axes, and nothing else: gravity does not appear in a specific-force measurement.
measurement model; the k column falls out of the model being linear in k
$$ h(\hat{\mathbf x}) = \begin{bmatrix} \boldsymbol\omega_b \\[2pt] R(\mathbf q)^{\mathsf T}\dfrac{k\,\mathbf F_{\text{aero}}}{m} \end{bmatrix}, \qquad
H_{\boldsymbol\omega} = I_3, \qquad
H_{k} = \frac{h_{\text{acc}}}{k} $$
$$ S = H P^- H^{\mathsf T} + R, \qquad
K = P^- H^{\mathsf T} S^{-1}, \qquad
\hat{\mathbf x}^+ = \hat{\mathbf x}^- + K\left(\mathbf z - h\right) $$
Joseph form keeps P positive definite
$$ P^+ = (I - KH)\,P^-\,(I - KH)^{\mathsf T} + K R K^{\mathsf T} $$
The quaternion is renormalised after every update. The residual is
dominated by aero model error, not by the sensor.
Initialisation
The filter starts from a perturbed estimate, and only the calibration residual is left
in the measurement.
residual bias in two horizontal axes tilts the gravity direction; the small-angle quaternion is half the angle
$$ \sigma_{\text{tilt}} = \arcsin\!\left(\frac{\sqrt{2}\,b_{\text{res}}}{g}\right),
\qquad \sigma_q = \tfrac12 \sigma_{\text{tilt}} $$
Velocity is seeded from the release estimate with a 0.01m/s per-axis error,
$k$ at 1 with a 0.1 standard deviation, and position at the
known release point.
