Extended Kalman Filter (EKF)

Nonlinear dynamics, nonlinear measurement models, first-order linearisation, Jacobians, state and covariance propagation, innovation, Kalman correction, spacecraft navigation applications, consistency, limitations, and practical EKF implementation.

4.2.7 Extended Kalman Filter (EKF)

The Extended Kalman Filter (EKF) extends the Kalman Filter framework to systems whose dynamics and/or measurements are nonlinear.

The classical Kalman Filter developed in Section 4.2.6 assumes

\[ \mathbf x_k = \mathbf F_{k-1}\mathbf x_{k-1} + \mathbf B_{k-1}\mathbf u_{k-1} + \mathbf w_{k-1} \]

and

\[ \mathbf z_k = \mathbf H_k\mathbf x_k + \mathbf v_k. \]

These equations assume that both the state evolution and measurement relationship can be represented using linear matrix operations.

Many real spacecraft systems cannot be represented this way.

For example, orbital motion is governed by nonlinear gravitational dynamics,

\[ \ddot{\mathbf r} = -\frac{\mu}{\|\mathbf r\|^3}\mathbf r, \]

a LiDAR range measurement depends nonlinearly on relative position,

\[ \rho = \sqrt{x^2+y^2+z^2}, \]

a camera may measure bearing angles such as

\[ \alpha = \tan^{-1}\left(\frac{y}{x}\right), \]

and spacecraft attitude evolves through nonlinear quaternion kinematics.

Therefore, spacecraft navigation frequently requires models of the form

\[ \boxed{ \mathbf x_k = f(\mathbf x_{k-1},\mathbf u_{k-1}) + \mathbf w_{k-1} } \]

and

\[ \boxed{ \mathbf z_k = h(\mathbf x_k) + \mathbf v_k }. \]

The EKF retains the fundamental Kalman-filter philosophy,

\[ \boxed{ \text{Prediction} \rightarrow \text{Measurement Comparison} \rightarrow \text{Uncertainty-Weighted Correction} } \]

but handles nonlinear functions by linearising them locally around the current state estimate.

This is the central idea of the Extended Kalman Filter.

4.2.7.1 Why the Extended Kalman Filter Is Needed

The classical KF works naturally when the state and measurement models are linear.

But consider a spacecraft orbiting Earth.

Its acceleration is approximately

\[ \mathbf a = -\frac{\mu}{r^3}\mathbf r. \]

Because

\[ r=\sqrt{x^2+y^2+z^2}, \]

the acceleration is a nonlinear function of position.

Similarly, during rendezvous and proximity operations, a camera may not directly provide Cartesian relative position

\[ [x,y,z]^T. \]

Instead, it may measure quantities such as azimuth and elevation:

\[ \alpha = \tan^{-1}\left(\frac{y}{x}\right) \]

and

\[ \beta = \tan^{-1} \left( \frac{z}{\sqrt{x^2+y^2}} \right). \]

A LiDAR may measure

\[ \rho = \sqrt{x^2+y^2+z^2}. \]

These relationships cannot generally be written as

\[ z=Hx \]

using a constant linear measurement matrix.

Therefore,

\[ \boxed{ \text{Real Spacecraft Navigation} \rightarrow \text{Nonlinear Dynamics and Measurements} } \]

which motivates

\[ \boxed{\text{Extended Kalman Filter}}. \]

4.2.7.2 From KF to EKF

The difference between the KF and EKF can be seen immediately from their models.

4.2.7.2.1 Linear Kalman Filter

\[ \mathbf x_k = F_{k-1}\mathbf x_{k-1} + B_{k-1}u_{k-1} + w_{k-1} \]

\[ \mathbf z_k = H_k\mathbf x_k + v_k. \]

4.2.7.2.2 Extended Kalman Filter

\[ \boxed{ \mathbf x_k = f(\mathbf x_{k-1},u_{k-1}) + w_{k-1} } \]

\[ \boxed{ \mathbf z_k = h(\mathbf x_k) + v_k }. \]

The important difference is that

\[ F x \]

is replaced by the nonlinear function

\[ f(x,u), \]

while

\[ Hx \]

is replaced by

\[ h(x). \]

However, covariance cannot simply be propagated through arbitrary nonlinear functions using the standard KF matrix equations.

The EKF therefore introduces local linearisation.

4.2.7.3 Central Idea of the EKF

The EKF does not make the physical system linear.

Instead, it approximates the nonlinear system as linear in a small neighbourhood around the current estimated state.

Suppose

\[ y=f(x). \]

Around some operating point x_0,

\[ f(x) \]

can be approximated using a first-order Taylor expansion:

\[ f(x) \approx f(x_0) + \left. \frac{\partial f}{\partial x} \right|_{x_0} (x-x_0). \]

The derivative tells us how a small change in the state produces a change in the nonlinear function.

For multidimensional spacecraft states, these derivatives form a matrix called the Jacobian.

Thus,

\[ \boxed{ \text{Nonlinear Model} \xrightarrow{\text{Local Linearisation}} \text{Jacobian-Based Local Model} } \]

and this local model is used to propagate uncertainty.

4.2.7.4 Nonlinear Process Model

The discrete nonlinear state model can be written as

\[ \boxed{ \mathbf x_k = f( \mathbf x_{k-1}, \mathbf u_{k-1} ) + \mathbf w_{k-1} } \]

where

The process noise is typically modelled as

\[ \mathbf w_k \sim \mathcal N(0,Q_k). \]

The state can contain quantities such as

\[ \mathbf x = \begin{bmatrix} \mathbf r\\ \mathbf v\\ \mathbf q\\ \boldsymbol\omega\\ \mathbf b_g\\ \mathbf b_a\\ \cdots \end{bmatrix}. \]

Depending on the application, an EKF may estimate orbit states, relative-navigation states, sensor biases, clock states, calibration parameters, or other uncertain quantities.

4.2.7.5 Continuous-Time Nonlinear Dynamics

Many spacecraft models are naturally written in continuous time:

\[ \boxed{ \dot{\mathbf x} = f( \mathbf x, \mathbf u, t ) + G\mathbf w }. \]

For example, an orbital navigation state may be

\[ \mathbf x = \begin{bmatrix} \mathbf r\\ \mathbf v \end{bmatrix} \]

with

\[ \dot{\mathbf r} = \mathbf v \]

and

\[ \dot{\mathbf v} = -\frac{\mu}{r^3}\mathbf r + \mathbf a_{pert} + \mathbf a_{control}. \]

Here, \mathbf a_{pert} may include effects such as atmospheric drag, J_2, solar radiation pressure, third-body gravity, or other perturbations.

The EKF propagates the nonlinear nominal state, rather than replacing these dynamics with a globally linear model.

4.2.7.6 Nonlinear Measurement Model

The measurement model is

\[ \boxed{ \mathbf z_k = h( \mathbf x_k ) + \mathbf v_k } \]

with

\[ \mathbf v_k \sim \mathcal N(0,R_k). \]

The function h(\cdot) converts the spacecraft state into the quantity that a particular sensor should observe.

This is especially important in spacecraft navigation because the estimator state and sensor output may represent very different physical quantities.

For example:

\[ \mathbf x = [x,y,z,v_x,v_y,v_z]^T \]

may be the relative Cartesian state, while a camera measures only

\[ z= [\alpha,\beta]^T. \]

The measurement function therefore performs the nonlinear mapping

\[ \boxed{ \text{Cartesian Relative State} \xrightarrow{h(x)} \text{Camera Angles} }. \]

4.2.7.7 Why the EKF Needs Jacobians

In the ordinary KF, covariance propagation uses

\[ P_k^- = F P_{k-1}^+F^T+Q. \]

For a nonlinear model,

\[ x_k=f(x_{k-1}), \]

there may be no constant matrix F.

The EKF therefore computes a local sensitivity matrix

\[ \boxed{ F_k = \left. \frac{\partial f}{\partial x} \right|_{\hat x} }. \]

Similarly, for the nonlinear measurement function

\[ z=h(x), \]

the EKF computes

\[ \boxed{ H_k = \left. \frac{\partial h}{\partial x} \right|_{\hat x} }. \]

These are the process Jacobian and measurement Jacobian.

They are among the most important quantities in EKF implementation.

4.2.7.8 What Is a Jacobian Matrix?

Suppose

\[ f(x) = \begin{bmatrix} f_1(x_1,x_2,\ldots,x_n)\\ f_2(x_1,x_2,\ldots,x_n)\\ \vdots\\ f_m(x_1,x_2,\ldots,x_n) \end{bmatrix}. \]

Then its Jacobian is

\[ \boxed{ J_f = \frac{\partial f}{\partial x} = \begin{bmatrix} \frac{\partial f_1}{\partial x_1} & \frac{\partial f_1}{\partial x_2} & \cdots & \frac{\partial f_1}{\partial x_n} \\ \frac{\partial f_2}{\partial x_1} & \frac{\partial f_2}{\partial x_2} & \cdots & \frac{\partial f_2}{\partial x_n} \\ \vdots&\vdots&\ddots&\vdots\\ \frac{\partial f_m}{\partial x_1} & \frac{\partial f_m}{\partial x_2} & \cdots & \frac{\partial f_m}{\partial x_n} \end{bmatrix} }. \]

Each element answers:

If this particular state variable changes slightly, how much does this particular process or measurement output change?

Therefore the Jacobian is fundamentally a local sensitivity matrix.

4.2.7.9 First-Order Linearisation

Suppose the true state is

\[ x=\hat x+\delta x. \]

Using Taylor expansion,

\[ f(\hat x+\delta x) = f(\hat x) + F\delta x + \text{higher-order terms}. \]

The EKF neglects the higher-order terms:

\[ \boxed{ f(\hat x+\delta x) \approx f(\hat x)+F\delta x }. \]

Similarly,

\[ h(\hat x+\delta x) \approx h(\hat x)+H\delta x. \]

This approximation is why the EKF is called extended rather than simply nonlinear Kalman filtering.

The Kalman equations are applied to the locally linearized error behaviour.

4.2.7.10 State Prediction in the EKF

The EKF propagates the state through the actual nonlinear process function:

\[ \boxed{ \hat{\mathbf x}_k^- = f( \hat{\mathbf x}_{k-1}^+, \mathbf u_{k-1} ) }. \]

This distinction is fundamental.

The EKF does not normally calculate

\[ \hat x_k^-=F\hat x_{k-1}^+ \]

as the primary nonlinear state propagation.

Instead,

\[ \boxed{ \text{State} \rightarrow \text{Propagated Through }f(\cdot) } \]

while

\[ \boxed{ \text{Covariance} \rightarrow \text{Propagated Using the Jacobian} }. \]

This is one of the most important EKF concepts.

4.2.7.11 Process Jacobian

For the discrete nonlinear model

\[ x_k=f(x_{k-1},u_{k-1}), \]

the state-transition Jacobian is

\[ \boxed{ F_{k-1} = \left. \frac{\partial f}{\partial x} \right|_ {\hat x_{k-1}^{+},u_{k-1}} }. \]

It describes how small errors in the previous state propagate into errors at the next step.

If

\[ \delta x_{k-1} = x_{k-1}-\hat x_{k-1}, \]

then locally,

\[ \boxed{ \delta x_k \approx F_{k-1}\delta x_{k-1} }. \]

Thus, F is not merely a mathematical requirement—it represents the local error dynamics of the nonlinear system.

4.2.7.12 Covariance Prediction

Once the Jacobian has been evaluated, the covariance is propagated as

\[ \boxed{ P_k^- = F_{k-1} P_{k-1}^+ F_{k-1}^T + Q_{k-1} }. \]

This looks identical to the KF covariance equation.

The important difference is that

\[ F_{k-1} \]

is now a Jacobian evaluated around the current estimated trajectory.

Therefore,

\[ \boxed{ F_k=F(\hat x_k) } \]

and can change continuously as the spacecraft moves.

4.2.7.13 Continuous-Time Covariance Propagation

For a continuous nonlinear system,

\[ \dot x=f(x,u)+Gw, \]

define

\[ \boxed{ A = \left. \frac{\partial f}{\partial x} \right|_{\hat x} }. \]

The covariance may then satisfy

\[ \boxed{ \dot P = AP + PA^T + GQ_cG^T }. \]

This is the continuous-time covariance propagation equation.

For a digital estimator, this model must be discretized to obtain an appropriate state-transition matrix and discrete process covariance.

A simple approximation is

\[ \Phi \approx I+A\Delta t, \]

but more accurate methods may use matrix exponentials:

\[ \boxed{ \Phi = e^{A\Delta t} }. \]

4.2.7.14 Predicted Measurement

After propagating the state, the EKF predicts what the sensor should observe.

Unlike the linear KF,

\[ \hat z=H\hat x^-, \]

the EKF uses the full nonlinear measurement model:

\[ \boxed{ \hat z_k = h( \hat x_k^- ) }. \]

For example, if LiDAR measures range,

\[ \hat\rho = \sqrt{ \hat x^2+ \hat y^2+ \hat z^2 }. \]

For a camera bearing measurement,

\[ \hat\alpha = \tan^{-1} \left( \frac{\hat y}{\hat x} \right). \]

Thus the predicted measurement is generated in the actual sensor measurement space.

4.2.7.15 Measurement Jacobian

The measurement Jacobian is

\[ \boxed{ H_k = \left. \frac{\partial h}{\partial x} \right|_{\hat x_k^-} }. \]

It describes how small changes in the estimated state affect the predicted sensor measurement.

If

\[ z=h(x), \]

then locally,

\[ \delta z \approx H\delta x. \]

The measurement Jacobian therefore determines how information from a particular sensor can correct different estimator states.

4.2.7.16 Example — LiDAR Range Jacobian

Suppose the relative position is

\[ \mathbf r= \begin{bmatrix} x\\y\\z \end{bmatrix} \]

and LiDAR measures

\[ \rho = \sqrt{x^2+y^2+z^2}. \]

Then

\[ h(x,y,z)=\sqrt{x^2+y^2+z^2}. \]

The Jacobian is

\[ H = \begin{bmatrix} \frac{\partial\rho}{\partial x} & \frac{\partial\rho}{\partial y} & \frac{\partial\rho}{\partial z} \end{bmatrix}. \]

Therefore,

\[ \boxed{ H = \begin{bmatrix} \frac{x}{\rho} & \frac{y}{\rho} & \frac{z}{\rho} \end{bmatrix} }. \]

This reveals something physically important:

A single range measurement constrains the spacecraft state primarily along the line of sight.

It does not independently determine all three Cartesian components.

Thus the Jacobian also helps expose measurement geometry and observability.

4.2.7.17 Example — Camera Bearing Jacobian

Consider

\[ \alpha = \tan^{-1}\left(\frac{y}{x}\right). \]

Then

\[ \frac{\partial\alpha}{\partial x} = -\frac{y}{x^2+y^2} \]

and

\[ \frac{\partial\alpha}{\partial y} = \frac{x}{x^2+y^2}. \]

Therefore,

\[ \boxed{ H_\alpha = \begin{bmatrix} -\frac{y}{x^2+y^2} & \frac{x}{x^2+y^2} & 0 \end{bmatrix} }. \]

Again, the measurement sensitivity changes with spacecraft geometry.

This means

\[ \boxed{ \text{Sensor Geometry} \rightarrow H \rightarrow \text{Information Content} \rightarrow \text{Estimation Performance} }. \]

This relationship is extremely important for RPO navigation.

4.2.7.18 Innovation in the EKF

The innovation remains

\[ \boxed{ y_k = z_k-\hat z_k } \]

but the predicted measurement is nonlinear:

\[ \boxed{ y_k = z_k - h(\hat x_k^-) }. \]

The innovation therefore represents the difference between

\[ \boxed{\text{Actual Sensor Output}} \]

and

\[ \boxed{\text{Sensor Output Predicted from the Current State Estimate}}. \]

This is one reason correct sensor modelling is essential.

If h(x) is incorrect, the residual can appear abnormal even when the actual sensor is functioning correctly.

4.2.7.19 Innovation Covariance

The EKF innovation covariance is

\[ \boxed{ S_k = H_kP_k^-H_k^T + R_k }. \]

Although this equation looks identical to the linear KF equation, H_k is now evaluated from the nonlinear measurement model at the current predicted state.

Therefore,

\[ \boxed{ S_k = \text{Locally Projected State Uncertainty} + \text{Sensor Uncertainty} }. \]

4.2.7.20 Kalman Gain

The EKF gain is

\[ \boxed{ K_k = P_k^-H_k^TS_k^{-1} }. \]

The same uncertainty-weighting principle from the KF remains.

If the measurement is accurate relative to the predicted uncertainty, it receives stronger influence.

If the measurement is uncertain, it receives less influence.

Thus,

\[ \boxed{ \text{EKF} = \text{Nonlinear Prediction} + \text{Locally Linearized Uncertainty Weighting} }. \]

4.2.7.21 State Measurement Update

The corrected estimate is

\[ \boxed{ \hat x_k^+ = \hat x_k^- + K_ky_k }. \]

Substituting the nonlinear innovation,

\[ \boxed{ \hat x_k^+ = \hat x_k^- + K_k \left[ z_k-h(\hat x_k^-) \right] }. \]

Therefore the correction still has the familiar Kalman structure:

\[ \boxed{ \text{Posterior} = \text{Prior} + \text{Gain} \times \text{Innovation} }. \]

4.2.7.22 Posterior Covariance Update

A common covariance update is

\[ \boxed{ P_k^+ = (I-K_kH_k)P_k^- }. \]

For improved numerical robustness, the Joseph form may be used:

\[ \boxed{ P_k^+ = (I-KH)P^-(I-KH)^T + KRK^T }. \]

As in the linear KF, a valid informative measurement generally reduces uncertainty in the observable directions.

4.2.7.23 Complete EKF Algorithm

The complete EKF cycle can now be written clearly.

4.2.7.23.1 Step 1 — Nonlinear State Prediction

\[ \boxed{ \hat x_k^- = f( \hat x_{k-1}^+, u_{k-1} ) } \]

4.2.7.23.2 Step 2 — Evaluate Process Jacobian

\[ \boxed{ F_{k-1} = \left. \frac{\partial f}{\partial x} \right|_{\hat x_{k-1}^+} } \]

4.2.7.23.3 Step 3 — Covariance Prediction

\[ \boxed{ P_k^- = F_{k-1}P_{k-1}^+F_{k-1}^T + Q_{k-1} } \]

4.2.7.23.4 Step 4 — Predict Sensor Measurement

\[ \boxed{ \hat z_k = h(\hat x_k^-) } \]

4.2.7.23.5 Step 5 — Evaluate Measurement Jacobian

\[ \boxed{ H_k = \left. \frac{\partial h}{\partial x} \right|_{\hat x_k^-} } \]

4.2.7.23.6 Step 6 — Innovation

\[ \boxed{ y_k = z_k-h(\hat x_k^-) } \]

4.2.7.23.7 Step 7 — Innovation Covariance

\[ \boxed{ S_k = H_kP_k^-H_k^T+R_k } \]

4.2.7.23.8 Step 8 — Kalman Gain

\[ \boxed{ K_k = P_k^-H_k^TS_k^{-1} } \]

4.2.7.23.9 Step 9 — State Update

\[ \boxed{ \hat x_k^+ = \hat x_k^-+K_ky_k } \]

4.2.7.23.10 Step 10 — Covariance Update

\[ \boxed{ P_k^+ = (I-K_kH_k)P_k^- }. \]

This gives the fundamental EKF loop:

\[ \boxed{ \text{Nonlinear Predict} \rightarrow \text{Linearize} \rightarrow \text{Predict Measurement} \rightarrow \text{Linearize Measurement} \rightarrow \text{Innovation} \rightarrow \text{Correct} }. \]

4.2.7.24 EKF vs KF — The Most Important Difference

This deserves a dedicated comparison on the page.

OperationKalman FilterExtended Kalman Filter
Process model Fx+Bu f(x,u)
Measurement model Hx h(x)
State propagation Linear Nonlinear
Measurement prediction Linear Nonlinear
Covariance propagation FPF^T+Q Jacobian-based
Measurement sensitivity H Measurement Jacobian
Linearisation Not required Required
Main approximation Linear-Gaussian model First-order local approximation

The critical concept is:

\[ \boxed{ \text{EKF propagates the state nonlinearly but propagates uncertainty through a local linear approximation.} } \]

4.2.7.25 EKF and Spacecraft Orbital Navigation

Consider

\[ x= \begin{bmatrix} r\\ v \end{bmatrix}. \]

The nonlinear orbital dynamics may be

\[ \dot r=v \]

\[ \dot v = -\frac{\mu}{r^3}r + a_{pert}. \]

The EKF propagates the spacecraft trajectory using these nonlinear equations.

GNSS or ground-tracking measurements then update the state.

A representative flow is

\[ \boxed{\text{Nonlinear Orbit Propagation}} \]

\[ \downarrow \]

\[ \boxed{\text{Predicted Position and Velocity}} \]

\[ \downarrow \]

\[ \boxed{\text{GNSS / Tracking Measurements}} \]

\[ \downarrow \]

\[ \boxed{\text{EKF Correction}}. \]

4.2.7.26 EKF for Relative Navigation and RPO

The EKF is particularly important for spacecraft rendezvous and proximity operations.

A basic relative state may be

\[ \boxed{ x_{rel} = \begin{bmatrix} r_{rel}\\ v_{rel} \end{bmatrix} }. \]

A more complete state may contain

\[ x= \begin{bmatrix} r_{rel}\\ v_{rel}\\ \theta_{rel}\\ \omega_{rel}\\ b_g\\ b_a \end{bmatrix}. \]

Sensors may include:

The estimator may therefore perform

\[ \boxed{\text{Relative Dynamics Prediction}} \]

followed by asynchronous updates from different relative-navigation sensors.

4.2.7.27 EKF for Camera-Based Relative Navigation

Suppose a camera measures azimuth and elevation:

\[ z= \begin{bmatrix} \alpha\\ \beta \end{bmatrix}. \]

The state may contain

\[ x= [x,y,z,v_x,v_y,v_z]^T. \]

The camera measurement function becomes

\[ h(x) = \begin{bmatrix} \tan^{-1}(y/x)\\ \tan^{-1} \left( z/\sqrt{x^2+y^2} \right) \end{bmatrix}. \]

The EKF:

  1. propagates relative position and velocity,
  2. predicts the camera angles,
  3. compares predicted angles with observed angles,
  4. computes the angular innovation,
  5. uses the Jacobian to determine state sensitivity,
  6. corrects the Cartesian relative state.

Thus,

\[ \boxed{ \text{Angular Measurements} \rightarrow \text{EKF} \rightarrow \text{Cartesian Relative-State Estimate} }. \]

This is a powerful example of estimating states that are not directly measured.

4.2.7.28 EKF for LiDAR-Based RPO

A LiDAR can provide measurements such as:

\[ \rho = \sqrt{x^2+y^2+z^2}. \]

Depending on the sensor, it may additionally provide:

A range measurement alone does not provide complete Cartesian relative position.

But when combined with spacecraft dynamics and other measurements,

\[ \boxed{ \text{LiDAR} + \text{Camera} + \text{Relative Dynamics} \rightarrow \text{Relative State Estimate} }. \]

The EKF provides one framework for this fusion.

4.2.7.29 EKF for GNSS Navigation

GNSS may provide position and velocity measurements.

A state could include

\[ x= \begin{bmatrix} r\\ v\\ b_c\\ \dot b_c \end{bmatrix} \]

where b_c and \dot b_c represent receiver clock bias and drift.

Depending on whether processed navigation solutions or raw pseudorange/Doppler measurements are used, the measurement model may be linear or nonlinear.

With raw GNSS observables, satellite geometry naturally produces nonlinear measurement equations, making EKF-based estimation especially useful.

4.2.7.30 EKF for IMU/GNSS Fusion

An inertial-navigation EKF may estimate

\[ x= \begin{bmatrix} r\\ v\\ \theta\\ b_g\\ b_a \end{bmatrix}. \]

The IMU provides high-rate measurements for propagation.

But inertial errors accumulate because of

\[ \boxed{ \text{Gyro Bias} + \text{Accelerometer Bias} + \text{Noise} + \text{Scale Factor} + \text{Misalignment} }. \]

GNSS periodically constrains position and velocity.

Hence,

\[ \boxed{\text{IMU}} \rightarrow \boxed{\text{High-Rate Nonlinear Propagation}} \]

while

\[ \boxed{\text{GNSS}} \rightarrow \boxed{\text{Long-Term Correction}}. \]

This is one of the classic EKF applications.

4.2.7.31 EKF and Spacecraft Attitude Estimation

Attitude dynamics are nonlinear.

Quaternion kinematics can be written as

\[ \boxed{ \dot q = \frac12\Omega(\omega)q }. \]

Measurements may come from:

A straightforward EKF can theoretically include quaternion components directly.

However, quaternion states have the constraint

\[ \boxed{ \|q\|=1 }. \]

This creates practical issues if quaternion components are treated as ordinary unconstrained Euclidean states.

For spacecraft attitude estimation, this motivates specialized formulations such as the

\[ \boxed{\text{Multiplicative Extended Kalman Filter — MEKF}}. \]

This should be introduced here but developed fully in the dedicated MEKF section.

4.2.7.32 EKF for Sensor Bias Estimation

The EKF can estimate sensor biases if they are included in the state and sufficiently observable.

For example,

\[ x= \begin{bmatrix} \cdots\\ b_g\\ b_a \end{bmatrix}. \]

A bias can be modelled as constant:

\[ \dot b=0 \]

or as a random walk:

\[ \boxed{ \dot b=w_b }. \]

The corresponding process-noise covariance describes expected bias evolution.

Therefore,

\[ \boxed{ \text{Sensor Bias} + \text{Bias Dynamic Model} + \text{Independent Measurement Information} \rightarrow \text{Bias Estimation} }. \]

Simply placing a bias in the state does not guarantee that it will be observable.

4.2.7.33 Multi-Rate and Asynchronous EKF

Real spacecraft sensors do not operate at identical rates.

For example:

\[ \text{IMU}:100~\text{Hz} \]

\[ \text{GNSS}:10~\text{Hz} \]

\[ \text{Camera}:5~\text{Hz} \]

\[ \text{Star Tracker}:1~\text{Hz}. \]

The EKF may therefore propagate continuously or at a high rate while measurement updates occur whenever measurements arrive.

Conceptually,

\[ \boxed{\text{Propagate}} \rightarrow \boxed{\text{IMU}} \rightarrow \boxed{\text{Propagate}} \rightarrow \boxed{\text{GNSS Update}} \rightarrow \boxed{\text{Propagate}} \rightarrow \boxed{\text{Camera Update}}. \]

This is particularly relevant to RPO sensor fusion.

4.2.7.34 Time Synchronization and Delayed Measurements

Because the EKF evaluates

\[ h(\hat x_k^-) \]

at a particular epoch, a measurement must correspond to that same epoch.

A delayed camera frame or GNSS solution can otherwise create a false innovation.

Therefore practical EKF implementations must account for:

A correct measurement associated with the wrong time can behave like an incorrect measurement.

Thus,

\[ \boxed{ \text{Correct Value} + \text{Wrong Timestamp} = \text{Incorrect Estimator Information} }. \]

4.2.7.35 Innovation Gating in an EKF

As in the KF,

\[ y_k=z_k-h(\hat x_k^-) \]

and

\[ S_k=H_kP_k^-H_k^T+R_k. \]

A normalized innovation statistic can be calculated:

\[ \boxed{ \epsilon_k = y_k^TS_k^{-1}y_k }. \]

This can be used to determine whether a measurement is statistically compatible with the current estimate.

A practical navigation chain may therefore be

\[ \boxed{\text{Measurement}} \rightarrow \boxed{\text{Nonlinear Prediction}} \rightarrow \boxed{\text{Innovation}} \rightarrow \boxed{\text{NIS/Gating}} \rightarrow \boxed{\text{Accept/Reject}} \rightarrow \boxed{\text{EKF Update}}. \]

This is especially important for camera feature mismatches and LiDAR outliers during RPO.

4.2.7.36 Observability in the EKF

For nonlinear systems, observability is more complicated than the simple linear observability matrix introduced for the KF.

However, the same physical principle remains:

\[ \boxed{ \text{A State Can Be Estimated Only If Available Measurements and Dynamics Contain Information About It} }. \]

For example, a single bearing measurement may not immediately determine range.

But spacecraft motion changes the observation geometry.

Over time,

\[ \boxed{ \text{Dynamics} + \text{Changing Geometry} + \text{Repeated Bearings} } \]

may make range observable.

Therefore spacecraft manoeuvres can sometimes improve navigation observability.

This is an important connection between

\[ \boxed{\text{Guidance}} \]

and

\[ \boxed{\text{Navigation}}. \]

4.2.7.37 Why EKF Performance Depends on the Operating Point

The Jacobians are evaluated around the current estimate.

Therefore,

\[ F_k = F(\hat x_k) \]

and

\[ H_k = H(\hat x_k). \]

If the estimated state is close to truth, the local linear approximation may be accurate.

If the estimate is far from truth, the Jacobian may describe the wrong local behaviour.

Thus,

\[ \boxed{ \text{Poor Initial Estimate} \rightarrow \text{Poor Linearisation} \rightarrow \text{Poor Correction} \rightarrow \text{Possible Divergence} }. \]

This is one of the central limitations of the EKF.

4.2.7.38 First-Order Approximation Error

Taylor expansion is actually

\[ f(x) = f(\hat x) + F\delta x + \frac12 \delta x^T \nabla^2f \delta x +\cdots \]

but the EKF retains only

\[ f(x) \approx f(\hat x)+F\delta x. \]

Therefore the EKF neglects second-order and higher-order nonlinear effects.

When uncertainty is small and nonlinearities are mild, this may be an excellent approximation.

When uncertainty is large or the system is strongly nonlinear,

\[ \boxed{ \text{Higher-Order Terms} \not\approx0 } \]

and EKF accuracy can deteriorate.

4.2.7.39 EKF Is Not an Exact Nonlinear Bayesian Filter

This distinction should be explicit.

The EKF is often described as a nonlinear Kalman Filter, but it does not calculate the exact nonlinear Bayesian posterior.

After a nonlinear transformation, a Gaussian random variable generally does not remain exactly Gaussian.

The EKF approximates the state distribution using

\[ \boxed{ \text{Estimated Mean} + \text{Covariance} } \]

and locally linearizes the nonlinear functions.

Therefore,

\[ \boxed{ \text{EKF} = \text{Approximate Nonlinear Bayesian Estimator} }. \]

4.2.7.40 Important EKF Limitations

The main limitations should have their own section.

4.2.7.40.1 First-order linearisation

The EKF neglects higher-order nonlinear effects.

4.2.7.40.2 Jacobian dependence

Incorrect Jacobians can cause serious estimator errors.

4.2.7.40.3 Sensitivity to initialization

A poor initial estimate may produce poor local linearisation.

4.2.7.40.4 Possible divergence

Strong nonlinearities, incorrect models or poor uncertainty tuning can lead to divergence.

4.2.7.40.5 Gaussian approximation

Non-Gaussian or multimodal state distributions are poorly represented by a single mean and covariance.

4.2.7.40.6 Numerical issues

Poor conditioning, covariance loss of symmetry, or incorrect discretization can degrade performance.

4.2.7.40.7 Coordinate singularities

Certain parameterizations such as Euler angles can introduce singularities.

4.2.7.40.8 State constraints

Quaternion normalization and other physical constraints require careful handling.

4.2.7.41 Common EKF Implementation Errors

This section is particularly useful for your interactive GNC website because it connects theory with engineering practice.

Common errors include:

A mathematically correct EKF equation cannot compensate for an incorrect physical model.

4.2.7.42 Angle Innovation Must Be Handled Carefully

Suppose the predicted bearing is

\[ 179^\circ \]

while the measured bearing is

\[ -179^\circ. \]

Naively,

\[ y=-179^\circ-179^\circ=-358^\circ. \]

But physically, the difference is only

\[ 2^\circ. \]

Therefore angular innovations must be wrapped to the appropriate interval, such as

\[ [-\pi,\pi). \]

This is extremely important for:

4.2.7.43 Process Noise in an EKF

As in the KF,

\[ Q \]

represents uncertainty in the process model.

But in a nonlinear spacecraft estimator, constructing Q may require careful physical modelling.

Possible sources include:

A preferred workflow is

\[ \boxed{ \text{Physical Uncertainty} \rightarrow \text{Continuous Noise Model} \rightarrow \text{Noise Mapping} \rightarrow \text{Discrete }Q }. \]

4.2.7.44 Measurement Noise in an EKF

The measurement covariance

\[ R \]

should represent uncertainty in the sensor measurement space.

For example, if a camera provides

\[ z= [\alpha,\beta]^T, \]

then R should describe uncertainty in angular measurements.

If LiDAR measures range,

\[ R_\rho=\sigma_\rho^2. \]

If a relative-pose sensor provides position and attitude, R should represent the corresponding position and orientation uncertainty and any relevant correlations.

Thus,

\[ \boxed{ R \text{ belongs to measurement space, not automatically to state space.} } \]

4.2.7.45 EKF Consistency Testing

The same statistical tools introduced for the KF remain important.

4.2.7.45.1 Innovation

\[ y_k=z_k-h(\hat x_k^-). \]

4.2.7.45.2 NIS

\[ \boxed{ NIS = y_k^TS_k^{-1}y_k }. \]

4.2.7.45.3 State error when truth is available

\[ e_k=x_k-\hat x_k. \]

4.2.7.45.4 NEES

\[ \boxed{ NEES = e_k^TP_k^{-1}e_k }. \]

For spacecraft simulation and Monte Carlo verification, these quantities help determine whether the EKF covariance realistically represents actual estimation errors.

4.2.7.46 Monte Carlo Verification of an EKF

Because EKF behaviour depends on nonlinear dynamics, initial conditions, sensor geometry and noise, Monte Carlo testing is particularly important.

Randomize quantities such as:

Evaluate:

\[ \boxed{ \text{RMSE} } \]

\[ \boxed{ \text{NIS} } \]

\[ \boxed{ \text{NEES} } \]

\[ \boxed{ \text{Convergence Time} } \]

\[ \boxed{ \text{Divergence Rate} }. \]

For RPO, also evaluate estimator performance across different approach phases and sensor handovers.

4.2.7.47 EKF Architecture for Spacecraft RPO

A practical architecture may look like:

\[ \boxed{\text{Relative Orbital Dynamics}} \]

\[ \downarrow \]

\[ \boxed{\text{Nonlinear State Propagation}} \]

\[ \downarrow \]

\[ \boxed{\hat x^-,P^-} \]

while sensors provide:

\[ \boxed{\text{Relative GNSS}} \]

\[ \boxed{\text{Camera}} \]

\[ \boxed{\text{LiDAR}} \]

\[ \boxed{\text{IMU}} \]

\[ \downarrow \]

\[ \boxed{\text{Time Synchronization}} \]

\[ \downarrow \]

\[ \boxed{\text{Measurement Models }h_i(x)} \]

\[ \downarrow \]

\[ \boxed{\text{Jacobians }H_i} \]

\[ \downarrow \]

\[ \boxed{\text{Innovation + Gating}} \]

\[ \downarrow \]

\[ \boxed{\text{EKF Measurement Updates}} \]

\[ \downarrow \]

\[ \boxed{\hat x^+,P^+} \]

\[ \downarrow \]

\[ \boxed{\text{Guidance + Control}}. \]

This is the practical connection between sensor modelling, fusion, navigation and closed-loop GNC.

4.2.7.48 Sensor Handover During RPO

A rendezvous mission may use different sensors as range decreases:

\[ \boxed{\text{Far Range}} \rightarrow \boxed{\text{Relative GNSS}} \]

\[ \boxed{\text{Mid Range}} \rightarrow \boxed{\text{GNSS + Camera}} \]

\[ \boxed{\text{Close Range}} \rightarrow \boxed{\text{Camera + LiDAR}} \]

\[ \boxed{\text{Capture}} \rightarrow \boxed{\text{Precision Relative Pose}}. \]

The EKF must handle changing measurement functions,

\[ h_{GNSS}(x), \quad h_{camera}(x), \quad h_{LiDAR}(x), \]

with corresponding Jacobians

\[ H_{GNSS}, \quad H_{camera}, \quad H_{LiDAR}. \]

Therefore sensor handover is not simply switching sensor values. It is a change in the information entering the navigation estimator.

4.2.7.49 EKF in Simulink

For the website, I would explicitly show the implementation architecture: 


Previous Posterior State
          │
          ▼
Nonlinear Dynamics f(x,u)
          │
          ▼
Predicted State x⁻
          │
          ├──────────────► Evaluate Process Jacobian F
          │                         │
          │                         ▼
Previous P ───────────────► Covariance Prediction
                                    │
                                    ▼
                                   P⁻

Sensor Measurement z
          │
          ▼
Predicted Measurement h(x⁻)
          │
          ▼
Innovation y = z - h(x⁻)
          │
          ├──────────────► Evaluate Measurement Jacobian H
          │
          ▼
Innovation Covariance S
          │
          ▼
Kalman Gain K
          │
          ├────────────────────┐
          ▼                    ▼
    State Update        Covariance Update
          │                    │
          └─────────┬──────────┘
                    ▼
                 x⁺ , P⁺
                    │
                    ▼
              Next Time Step

And the estimator must receive

\[ \boxed{\text{Sensor Measurements}} \]

not

\[ \boxed{\text{Simulation Truth}}. \]

Truth should remain outside the estimator and be used only for performance evaluation.

4.2.7.50 Recommended EKF Software Blocks

For a clean implementation, separate:

\[ \boxed{\text{Nonlinear State Propagation}} \]

\[ \boxed{\text{Process Jacobian}} \]

\[ \boxed{\text{Covariance Propagation}} \]

\[ \boxed{\text{Sensor Preprocessing}} \]

\[ \boxed{\text{Nonlinear Measurement Model}} \]

\[ \boxed{\text{Measurement Jacobian}} \]

\[ \boxed{\text{Innovation}} \]

\[ \boxed{\text{Gating}} \]

\[ \boxed{\text{Kalman Gain}} \]

\[ \boxed{\text{State Update}} \]

\[ \boxed{\text{Covariance Update}} \]

\[ \boxed{\text{Estimator Health Monitoring}}. \]

This modular structure is easier to test than a single large EKF block.

4.2.7.51 EKF Verification Hierarchy

A good implementation should be verified progressively.

Start with:

\[ \boxed{\text{Known Linear Problem}} \]

where the EKF should reproduce KF behaviour.

Then test:

\[ \boxed{\text{Simple Nonlinear Dynamics}} \]

then:

\[ \boxed{\text{Nonlinear Measurement}} \]

then:

\[ \boxed{\text{Multiple Sensors}} \]

then:

\[ \boxed{\text{Sensor Dropout + Outliers}} \]

then:

\[ \boxed{\text{Monte Carlo}} \]

then:

\[ \boxed{\text{SIL}} \rightarrow \boxed{\text{HIL}}. \]

This makes debugging much easier.

4.2.7.52 When the EKF Works Well

The EKF is particularly effective when:

In these conditions, the EKF can provide excellent estimation performance with relatively modest computational cost.

This is one reason it remains widely used in aerospace navigation.

4.2.7.53 When the EKF Can Struggle

The EKF may perform poorly when:

\[ \boxed{\text{Strong Nonlinearity}} \]

\[ \boxed{\text{Large Initial Error}} \]

\[ \boxed{\text{Poor Observability}} \]

\[ \boxed{\text{Highly Non-Gaussian Uncertainty}} \]

\[ \boxed{\text{Multimodal State Distribution}} \]

\[ \boxed{\text{Incorrect Jacobians}} \]

\[ \boxed{\text{Incorrect }Q/R} \]

\[ \boxed{\text{Measurement Association Errors}}. \]

These limitations motivate other nonlinear estimation approaches.

4.2.7.54 EKF vs UKF — Conceptual Preview

The EKF handles nonlinear transformations by linearizing:

\[ \boxed{ f(x) \xrightarrow{\text{Jacobian}} \text{Local Linear Approximation} }. \]

The Unscented Kalman Filter takes a different approach.

Instead of calculating Jacobians, it selects representative sigma points and propagates them through the nonlinear function:

\[ \boxed{ \text{State Distribution} \rightarrow \text{Sigma Points} \rightarrow f(\cdot) \rightarrow \text{Reconstructed Mean and Covariance} }. \]

This avoids explicit first-order Jacobian linearisation.

The UKF will be developed later in the chapter.

4.2.7.55 EKF vs ESKF — Conceptual Preview

In a standard EKF, the full state may be estimated directly.

An Error-State Kalman Filter (ESKF) instead maintains a nonlinear nominal state and estimates a small error state:

\[ \boxed{ x_{true} = x_{nominal} \oplus \delta x }. \]

The filter estimates

\[ \delta x \]

rather than continuously treating the entire physical state as a small Euclidean correction.

This is especially useful for inertial navigation and attitude states.

The ESKF will therefore be treated separately in 4.2.8.

4.2.7.56 EKF vs MEKF — Conceptual Preview

For quaternion attitude estimation, directly adding a Kalman correction to a quaternion is inconvenient because

\[ \|q\|=1. \]

The MEKF represents the attitude estimate using a nominal quaternion while the filter estimates a small three-dimensional attitude error.

Conceptually,

\[ \boxed{ q_{true} = \delta q \otimes \hat q }. \]

This preserves the quaternion structure more naturally.

Therefore the MEKF is particularly important for spacecraft attitude determination.

4.2.7.57 Key Equations at a Glance

4.2.7.57.1 Nonlinear Process Model

\[ \boxed{ x_k=f(x_{k-1},u_{k-1})+w_{k-1} } \]

4.2.7.57.2 Nonlinear Measurement Model

\[ \boxed{ z_k=h(x_k)+v_k } \]

4.2.7.57.3 State Prediction

\[ \boxed{ \hat x_k^- = f(\hat x_{k-1}^+,u_{k-1}) } \]

4.2.7.57.4 Process Jacobian

\[ \boxed{ F_{k-1} = \left. \frac{\partial f}{\partial x} \right|_{\hat x_{k-1}^+} } \]

4.2.7.57.5 Covariance Prediction

\[ \boxed{ P_k^- = F_{k-1}P_{k-1}^+F_{k-1}^T+Q_{k-1} } \]

4.2.7.57.6 Predicted Measurement

\[ \boxed{ \hat z_k=h(\hat x_k^-) } \]

4.2.7.57.7 Measurement Jacobian

\[ \boxed{ H_k = \left. \frac{\partial h}{\partial x} \right|_{\hat x_k^-} } \]

4.2.7.57.8 Innovation

\[ \boxed{ y_k=z_k-h(\hat x_k^-) } \]

4.2.7.57.9 Innovation Covariance

\[ \boxed{ S_k=H_kP_k^-H_k^T+R_k } \]

4.2.7.57.10 Kalman Gain

\[ \boxed{ K_k=P_k^-H_k^TS_k^{-1} } \]

4.2.7.57.11 State Update

\[ \boxed{ \hat x_k^+ = \hat x_k^-+K_ky_k } \]

4.2.7.57.12 Covariance Update

\[ \boxed{ P_k^+ = (I-K_kH_k)P_k^- }. \]

4.2.7.58 The EKF in One Diagram

The conceptual structure should be emphasized visually:

\[ \boxed{\text{Previous State Estimate}} \]

\[ \downarrow \]

\[ \boxed{\text{Nonlinear Dynamics }f(x,u)} \]

\[ \downarrow \]

\[ \boxed{\text{Predicted State }\hat x^-} \]

\[ \downarrow \]

\[ \boxed{\text{Nonlinear Sensor Prediction }h(\hat x^-)} \]

\[ \downarrow \]

\[ \boxed{\text{Innovation }z-h(\hat x^-)} \]

\[ \downarrow \]

\[ \boxed{\text{Jacobian-Based Uncertainty Weighting}} \]

\[ \downarrow \]

\[ \boxed{\text{Corrected State }\hat x^+} \]

while in parallel,

\[ \boxed{P^+} \rightarrow \boxed{F} \rightarrow \boxed{P^-} \rightarrow \boxed{H,R} \rightarrow \boxed{K} \rightarrow \boxed{P^+}. \]

4.2.7.59 Key Takeaways

The reader should leave this page understanding these points:

The most important concept is:

\[ \boxed{ \text{EKF} = \text{Nonlinear State and Measurement Models} + \text{Local Jacobian Linearisation} + \text{Kalman Uncertainty Update} } \]

or, even more physically,

\[ \boxed{ \text{Propagate the spacecraft nonlinearly} \quad \text{but propagate uncertainty locally linearly.} } \]

4.2.7.60 Transition to 4.2.8 Error-State Kalman Filter

The EKF provides a practical solution for nonlinear spacecraft estimation, but one difficulty remains.

Some spacecraft states—particularly attitude, position, velocity and inertial sensor biases—are often more naturally represented using a nonlinear nominal state plus a small estimation error.

Instead of estimating the entire physical state correction directly,

\[ x \]

we can write

\[ \boxed{ x_{true} = x_{nominal} \oplus \delta x } \]

and let the Kalman Filter estimate only

\[ \boxed{\delta x}. \]

After each measurement update, the estimated error is injected into the nominal state and the error state is reset.

This leads to the next topic:

\[ \boxed{\textbf{4.2.8 Error-State Kalman Filter (ESKF)}} \]

where we develop nominal-state propagation, error-state dynamics, error injection, covariance reset, IMU propagation, gyro and accelerometer bias estimation, and spacecraft inertial-navigation applications.

Next: 4.2.8 Error-State Kalman Filter (ESKF)

The Extended Kalman Filter handles nonlinear dynamics and nonlinear measurements through local Jacobian linearisation. The next section develops the error-state formulation, where a nonlinear nominal state is propagated while the filter estimates only a small local error state.

4.2.8 Error-State Kalman Filter (ESKF)

Nominal-state propagation, error-state dynamics, error injection, covariance reset, IMU propagation, gyro and accelerometer bias estimation, and spacecraft inertial-navigation applications.

Continue to Error-State Kalman Filter →