Kalman Filter Fundamentals

Linear state-space models, state prediction, covariance propagation, innovation, innovation covariance, Kalman gain, measurement correction, posterior covariance, process and measurement noise, initialization, observability, consistency, multi-sensor fusion, RPO applications, and practical Kalman-filter implementation.

4.2.6 Kalman Filter Fundamentals

The Kalman Filter (KF) is one of the most important recursive state-estimation algorithms used in spacecraft Guidance, Navigation and Control. It combines a mathematical model of spacecraft dynamics with noisy sensor measurements to obtain an optimal state estimate under the standard linear-Gaussian assumptions.

The Bayesian framework developed in Section 4.2.5 established the general idea:

\[ \boxed{ \text{Prior} + \text{Measurement Information} \rightarrow \text{Posterior} } \]

The Kalman Filter converts this probabilistic principle into a computationally efficient recursive algorithm.

At every time step, the filter performs two fundamental operations:

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

followed by

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

or, conceptually,

\[ \boxed{ \text{Previous Estimate} \rightarrow \text{Predict} \rightarrow \text{Compare with Measurement} \rightarrow \text{Correct} \rightarrow \text{New Estimate} } \]

The Kalman Filter does not estimate only the spacecraft state. It simultaneously estimates how uncertain that state estimate is.

Its two fundamental outputs are therefore

\[ \boxed{\hat{\mathbf x}_k} \]

and

\[ \boxed{\mathbf P_k} \]

where \hat{\mathbf x}_k is the estimated state and \mathbf P_k is the state-estimation error covariance.

This distinction is fundamental in spacecraft navigation. Knowing that the estimated relative position is 10 cm is not sufficient during rendezvous or docking; the flight computer must also know whether the uncertainty is 1 mm, 1 cm, or 1 m.

4.2.6.1 Why the Kalman Filter Is Needed

A spacecraft navigation system generally has two imperfect sources of knowledge.

The first is the spacecraft dynamics model.

If the current state is known, equations of motion can predict where the spacecraft should be at the next time.

For example,

\[ \mathbf x_{k+1} = f(\mathbf x_k,\mathbf u_k). \]

But the model is never perfect.

Errors can arise from:

The second source is the sensor system.

Sensors provide observations of the actual spacecraft, but measurements contain:

Therefore,

\[ \boxed{ \text{Dynamics Model} = \text{Continuous but Imperfect Prediction} } \]

while

\[ \boxed{ \text{Sensor} = \text{Physical Observation but Noisy Measurement}. } \]

The Kalman Filter combines the strengths of both.

4.2.6.2 Fundamental Idea of Kalman Filtering

Suppose the dynamics predict that the spacecraft is at

\[ x=100\text{ m} \]

but a sensor reports

\[ z=103\text{ m}. \]

Which value should be used?

The answer depends on uncertainty.

If the prediction is highly accurate and the sensor is noisy, the estimate should remain closer to 100 m.

If the model prediction is uncertain but the sensor is highly accurate, the estimate should move closer to 103 m.

Therefore,

\[ \boxed{ \text{Correction Strength} = f( \text{Prediction Uncertainty}, \text{Measurement Uncertainty} ) } \]

The Kalman Filter computes this weighting mathematically through the Kalman gain.

Thus the KF is not a simple averaging algorithm.

\[ \boxed{ \text{Kalman Filtering} \neq \frac{\text{Prediction}+\text{Measurement}}{2} } \]

Instead,

\[ \boxed{ \text{Kalman Filtering} = \text{Uncertainty-Weighted State Estimation}. } \]

4.2.6.3 The State Vector

The first step is defining the quantities that must be estimated.

For a simple translational navigation problem,

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

For three-dimensional spacecraft navigation,

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

A more complete navigation state might contain

\[ \boxed{ \mathbf x= \begin{bmatrix} \mathbf r\\ \mathbf v\\ \boldsymbol\theta\\ \boldsymbol\omega\\ \mathbf b_g\\ \mathbf b_a \end{bmatrix} } \]

depending on the estimator formulation.

Possible states include:

A critical principle is:

\[ \boxed{ \text{A State Does Not Need to Be Directly Measured to Be Estimated} } \]

provided the system is sufficiently observable through the dynamics and available measurements.

4.2.6.4 Linear Discrete-Time State-Space Model

The classical Kalman Filter assumes a linear state-space model.

The process model is

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

where:

\[ \mathbf x_k \]

is the state at time k,

\[ \mathbf F_{k-1} \]

is the state-transition matrix,

\[ \mathbf u_{k-1} \]

is the known control input,

\[ \mathbf B_{k-1} \]

maps the control input into the state,

and

\[ \mathbf w_{k-1} \]

represents process noise.

The process noise is commonly assumed to be zero-mean Gaussian:

\[ \boxed{ \mathbf w_{k-1} \sim \mathcal N( \mathbf 0,\mathbf Q_{k-1} ) } \]

where \mathbf Q is the process-noise covariance matrix.

4.2.6.5 Physical Meaning of the State-Transition Matrix F

The matrix

\[ \mathbf F \]

describes how the state evolves from one discrete time step to the next.

Consider a one-dimensional constant-velocity model:

\[ x_{k+1} = x_k+v_k\Delta t \]

and

\[ v_{k+1}=v_k. \]

Then

\[ \begin{bmatrix} x_{k+1}\\ v_{k+1} \end{bmatrix} = \begin{bmatrix} 1 & \Delta t\\ 0 & 1 \end{bmatrix} \begin{bmatrix} x_k\\ v_k \end{bmatrix}. \]

Therefore,

\[ \boxed{ \mathbf F= \begin{bmatrix} 1 & \Delta t\\ 0 & 1 \end{bmatrix}. } \]

The off-diagonal term \Delta t tells us that velocity uncertainty will also influence future position uncertainty.

This is an important concept:

\[ \boxed{ \text{Dynamics Propagate Both the State and Relationships Between State Errors}. } \]

4.2.6.6 Control-Input Matrix B

If known forces, accelerations, or control commands affect the state, they can be represented using

\[ \mathbf B\mathbf u. \]

For a constant acceleration a,

\[ x_{k+1} = x_k+v_k\Delta t+\frac12a_k\Delta t^2 \]

and

\[ v_{k+1} = v_k+a_k\Delta t. \]

Thus

\[ \mathbf u_k=a_k \]

and

\[ \boxed{ \mathbf B= \begin{bmatrix} \frac12\Delta t^2\\ \Delta t \end{bmatrix}. } \]

In spacecraft applications, control input might represent:

The exact interpretation depends on the navigation formulation.

4.2.6.7 Measurement Model

The second major equation is the sensor measurement model:

\[ \boxed{ \mathbf z_k = \mathbf H_k\mathbf x_k + \mathbf v_k } \]

where

\[ \mathbf z_k \]

is the measurement vector,

\[ \mathbf H_k \]

is the measurement matrix,

and

\[ \mathbf v_k \]

is measurement noise.

The noise is commonly modelled as

\[ \boxed{ \mathbf v_k \sim \mathcal N( \mathbf 0,\mathbf R_k ) } \]

where

\[ \mathbf R_k \]

is the measurement-noise covariance.

4.2.6.8 Physical Meaning of the Measurement Matrix H

The matrix

\[ \mathbf H \]

maps the state into the quantity measured by the sensor.

Suppose

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

but a sensor measures position only.

Then

\[ z=x+v_m \]

and

\[ \boxed{ \mathbf H= \begin{bmatrix} 1&0 \end{bmatrix}. } \]

If the sensor measures velocity only,

\[ \boxed{ \mathbf H= \begin{bmatrix} 0&1 \end{bmatrix}. } \]

If both position and velocity are measured,

\[ \boxed{ \mathbf H= \begin{bmatrix} 1&0\\ 0&1 \end{bmatrix}. } \]

Therefore H tells the estimator:

Which combination of the state variables should this sensor observe?

4.2.6.9 State Estimate Notation: Prior and Posterior

The Kalman Filter maintains two estimates at each measurement epoch.

The prior, or predicted estimate, is

\[ \boxed{ \hat{\mathbf x}_k^{-} } \]

and represents knowledge before processing measurement z_k.

The posterior, or corrected estimate, is

\[ \boxed{ \hat{\mathbf x}_k^{+} } \]

and represents knowledge after processing the measurement.

Similarly,

\[ \boxed{ \mathbf P_k^{-} } \]

is the predicted covariance, while

\[ \boxed{ \mathbf P_k^{+} } \]

is the posterior covariance.

The complete cycle is therefore

\[ \boxed{ (\hat{\mathbf x}_{k-1}^{+},\mathbf P_{k-1}^{+}) } \]

\[ \downarrow \]

Prediction

\[ \downarrow \]

\[ \boxed{ (\hat{\mathbf x}_k^{-},\mathbf P_k^{-}) } \]

\[ \downarrow \]

Measurement Update

\[ \downarrow \]

\[ \boxed{ (\hat{\mathbf x}_k^{+},\mathbf P_k^{+}) }. \]

4.2.6.10 Step 1 — State Prediction

The state prediction is

\[ \boxed{ \hat{\mathbf x}_k^{-} = \mathbf F_{k-1}\hat{\mathbf x}_{k-1}^{+} + \mathbf B_{k-1}\mathbf u_{k-1}. } \]

This answers:

Based on the previous best estimate and our dynamic model, where do we expect the spacecraft to be now?

No new measurement has yet been used.

For spacecraft navigation this could mean propagating:

4.2.6.11 Step 2 — Covariance Prediction

State prediction alone is insufficient.

The estimator must also propagate uncertainty:

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

This is one of the most important equations in the Kalman Filter.

The first term,

\[ \mathbf F \mathbf P^{+} \mathbf F^T, \]

propagates the previous uncertainty through the system dynamics.

The second term,

\[ \mathbf Q, \]

adds uncertainty introduced by imperfect process modelling.

Thus,

\[ \boxed{ \text{Predicted Uncertainty} = \text{Propagated Previous Uncertainty} + \text{New Model Uncertainty}. } \]

4.2.6.12 Why Covariance Can Grow During Propagation

Imagine a spacecraft whose position and velocity are estimated at time t_0.

If the spacecraft receives no navigation measurements for several minutes, the estimator can still propagate the orbit.

However, disturbances and model imperfections accumulate.

Therefore the estimated state continues to exist, but confidence decreases.

Mathematically,

\[ \mathbf P^{-} \]

usually grows.

This represents a physically meaningful principle:

\[ \boxed{ \text{No Measurement} \not\Rightarrow \text{No Estimate} } \]

but

\[ \boxed{ \text{No Measurement} \Rightarrow \text{Increasing Uncertainty} } \]

in the usual case.

4.2.6.13 Understanding Process Noise Q

The matrix

\[ \boxed{\mathbf Q} \]

represents uncertainty in state evolution.

It may account for:

A very small Q tells the filter:

Trust the dynamic model strongly.

A large Q tells the filter:

The model prediction may be inaccurate.

Incorrect Q can significantly affect estimator behaviour.

4.2.6.14 Q Is Not Simply a Tuning Knob

Although Q is often tuned during development, it should ideally originate from a physical stochastic model.

For example, if acceleration uncertainty is represented by white noise with spectral density q_a, then the discrete covariance for a constant-velocity model has the familiar structure

\[ \boxed{ \mathbf Q = q_a \begin{bmatrix} \frac{\Delta t^3}{3} & \frac{\Delta t^2}{2}\\ \frac{\Delta t^2}{2} & \Delta t \end{bmatrix} } \]

for the corresponding continuous white-acceleration model.

Other modelling conventions can produce different discrete Q forms.

Therefore the chosen process model must be clear.

A good engineering workflow is

\[ \boxed{ \text{Physical Disturbance Model} \rightarrow \text{Continuous Stochastic Model} \rightarrow \text{Discretization} \rightarrow Q_d. } \]

4.2.6.15 Measurement Prediction

Once the state has been predicted, the filter determines what measurement should be expected.

For a linear measurement model,

\[ \boxed{ \hat{\mathbf z}_k = \mathbf H_k \hat{\mathbf x}_k^{-}. } \]

This is called the predicted measurement.

It answers:

If our predicted spacecraft state were correct, what should the sensor currently measure?

This quantity is then compared with the actual sensor measurement.

4.2.6.16 Innovation or Measurement Residual

The difference between the actual measurement and predicted measurement is

\[ \boxed{ \mathbf y_k = \mathbf z_k - \mathbf H_k\hat{\mathbf x}_k^{-}. } \]

The vector

\[ \boxed{\mathbf y_k} \]

is called the:

Conceptually,

\[ \boxed{ \text{Innovation} = \text{Actual Measurement} - \text{Expected Measurement}. } \]

If

\[ \mathbf y_k\approx0, \]

the measurement agrees well with the prediction.

A large innovation means that the measurement and prediction disagree.

However, a large innovation alone does not prove that the sensor is faulty.

Its significance depends on the expected innovation uncertainty.

4.2.6.17 Innovation Covariance

The covariance of the innovation is

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

This equation combines two sources of uncertainty.

The term

\[ \mathbf H \mathbf P^{-} \mathbf H^T \]

represents uncertainty in the predicted measurement caused by state uncertainty.

The term

\[ \mathbf R \]

represents sensor measurement uncertainty.

Thus,

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

This is crucial because an innovation of 1 m may be enormous for one sensor configuration but perfectly normal for another.

4.2.6.18 Why Innovation Covariance Matters

Suppose the innovation is

\[ y=1\text{ m}. \]

If the expected standard deviation is

\[ \sigma_y=0.01\text{ m}, \]

then the innovation is extremely unusual.

But if

\[ \sigma_y=10\text{ m}, \]

the same innovation is insignificant.

Therefore the estimator should not interpret residual magnitude without considering covariance.

This leads later to normalized innovation tests such as

\[ \boxed{ \epsilon_k = \mathbf y_k^T \mathbf S_k^{-1} \mathbf y_k } \]

which forms the basis of Normalized Innovation Squared (NIS) and Mahalanobis-distance gating.

4.2.6.19 Kalman Gain

The Kalman gain is

\[ \boxed{ \mathbf K_k = \mathbf P_k^{-} \mathbf H_k^T \mathbf S_k^{-1} } \]

or equivalently,

\[ \boxed{ \mathbf K_k = \mathbf P_k^{-} \mathbf H_k^T \left( \mathbf H_k \mathbf P_k^{-} \mathbf H_k^T + \mathbf R_k \right)^{-1}. } \]

The Kalman gain determines how strongly the innovation changes the state estimate.

Conceptually,

\[ \boxed{ \text{Kalman Gain} = \text{Optimal Measurement Correction Weight} } \]

under the classical assumptions.

4.2.6.20 Physical Interpretation of the Kalman Gain

Consider the scalar case where the state is measured directly.

Then

\[ H=1 \]

and

\[ \boxed{ K = \frac{P^-}{P^-+R}. } \]

This equation makes the interpretation clear.

If

\[ P^-\gg R, \]

the prediction is much more uncertain than the sensor.

Then

\[ K\rightarrow1. \]

The filter strongly trusts the measurement.

If

\[ R\gg P^-, \]

the sensor is much noisier than the prediction.

Then

\[ K\rightarrow0. \]

The filter trusts the prediction more strongly.

Therefore,

\[ \boxed{ P^- \uparrow \Rightarrow \text{Measurement Influence} \uparrow } \]

and

\[ \boxed{ R \uparrow \Rightarrow \text{Measurement Influence} \downarrow. } \]

4.2.6.21 Kalman Gain Is Not a Fixed Gain

Unlike a conventional fixed-gain observer, the Kalman gain generally changes with time:

\[ \boxed{\mathbf K_k}. \]

Why?

Because the filter continuously updates uncertainty.

As

\[ \mathbf P_k \]

changes, so does

\[ \mathbf K_k. \]

If measurement quality changes and R_k is updated, the gain also changes.

Therefore the Kalman Filter automatically adjusts how strongly measurements are used according to the estimated uncertainty.

This is one of its major advantages over fixed-gain correction methods.

4.2.6.22 State Measurement Update

Once the innovation and Kalman gain are available, the corrected state is

\[ \boxed{ \hat{\mathbf x}_k^{+} = \hat{\mathbf x}_k^{-} + \mathbf K_k\mathbf y_k. } \]

Substituting the innovation,

\[ \boxed{ \hat{\mathbf x}_k^{+} = \hat{\mathbf x}_k^{-} + \mathbf K_k \left( \mathbf z_k - \mathbf H_k\hat{\mathbf x}_k^{-} \right). } \]

This equation has an intuitive structure:

\[ \boxed{ \text{Corrected Estimate} = \text{Predicted Estimate} + \text{Weighted Measurement Disagreement}. } \]

The innovation determines what correction is needed.

The Kalman gain determines how much correction should be applied.

4.2.6.23 Posterior Covariance Update

After incorporating the measurement, uncertainty must also be updated.

The common covariance update is

\[ \boxed{ \mathbf P_k^{+} = (\mathbf I-\mathbf K_k\mathbf H_k) \mathbf P_k^{-}. } \]

This reflects the information gained from the measurement.

Usually,

\[ \boxed{ \mathbf P_k^{+} \preceq \mathbf P_k^{-} } \]

in the positive-semidefinite sense for the ideal update.

Thus:

\[ \boxed{ \text{Valid Informative Measurement} \rightarrow \text{Reduced Uncertainty}. } \]

4.2.6.24 Joseph-Form Covariance Update

For numerical implementation, a more robust form is often preferred:

\[ \boxed{ \mathbf P_k^{+} = (\mathbf I-\mathbf K_k\mathbf H_k) \mathbf P_k^{-} (\mathbf I-\mathbf K_k\mathbf H_k)^T + \mathbf K_k \mathbf R_k \mathbf K_k^T. } \]

This is the Joseph form.

In exact arithmetic, it is equivalent to the simplified update when K is the Kalman gain.

In finite-precision computation, the Joseph form helps preserve covariance symmetry and positive-semidefinite behaviour.

For flight-quality software, numerical covariance management is important.

4.2.6.25 Complete Kalman Filter Cycle

The complete recursive algorithm can now be written compactly.

4.2.6.25.1 Prediction

State:

\[ \boxed{ \hat{\mathbf x}_k^{-} = \mathbf F_{k-1}\hat{\mathbf x}_{k-1}^{+} + \mathbf B_{k-1}\mathbf u_{k-1} } \]

Covariance:

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

4.2.6.25.2 Measurement Prediction

\[ \boxed{ \hat{\mathbf z}_k = \mathbf H_k\hat{\mathbf x}_k^{-} } \]

4.2.6.25.3 Innovation

\[ \boxed{ \mathbf y_k = \mathbf z_k-\hat{\mathbf z}_k } \]

4.2.6.25.4 Innovation Covariance

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

4.2.6.25.5 Kalman Gain

\[ \boxed{ \mathbf K_k = \mathbf P_k^{-} \mathbf H_k^T \mathbf S_k^{-1} } \]

4.2.6.25.6 State Update

\[ \boxed{ \hat{\mathbf x}_k^{+} = \hat{\mathbf x}_k^{-} + \mathbf K_k\mathbf y_k } \]

4.2.6.25.7 Covariance Update

\[ \boxed{ \mathbf P_k^{+} = (\mathbf I-\mathbf K_k\mathbf H_k) \mathbf P_k^{-} } \]

or using Joseph form,

\[ \boxed{ \mathbf P_k^{+} = (\mathbf I-\mathbf K_k\mathbf H_k) \mathbf P_k^{-} (\mathbf I-\mathbf K_k\mathbf H_k)^T + \mathbf K_k\mathbf R_k\mathbf K_k^T. } \]

Then the cycle repeats.

4.2.6.26 Complete Information Flow

The algorithm can be understood as:

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

\[ \downarrow \]

\[ \boxed{\text{Dynamics Model}} \]

\[ \downarrow \]

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

while simultaneously

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

\[ \downarrow \]

\[ \boxed{\text{Dynamics + Process Noise}} \]

\[ \downarrow \]

\[ \boxed{\text{Predicted Covariance}}. \]

Then:

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

and

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

produce

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

The predicted covariance and sensor covariance produce

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

and then

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

Finally,

\[ \boxed{ \text{Prediction} + \text{Kalman Gain}\times\text{Innovation} } \]

produces the posterior state estimate.

4.2.6.27 What P, Q, R, S, and K Mean

These matrices are central to Kalman filtering.

4.2.6.27.1 P: State-Estimation Error Covariance

\[ \boxed{ \mathbf P = E[ (\mathbf x-\hat{\mathbf x}) (\mathbf x-\hat{\mathbf x})^T ] } \]

It describes uncertainty in the estimated state.

4.2.6.27.2 Q: Process-Noise Covariance

\[ \boxed{\mathbf Q} \]

describes uncertainty introduced by the dynamic/process model.

4.2.6.27.3 R: Measurement-Noise Covariance

\[ \boxed{\mathbf R} \]

describes uncertainty in sensor measurements.

4.2.6.27.4 S: Innovation Covariance

\[ \boxed{ \mathbf S = \mathbf H\mathbf P^-\mathbf H^T+\mathbf R } \]

describes expected uncertainty in the measurement residual.

4.2.6.27.5 K: Kalman Gain

\[ \boxed{\mathbf K} \]

determines how measurement information corrects the predicted state.

A useful conceptual chain is

\[ \boxed{ P,Q,R \rightarrow S \rightarrow K \rightarrow \text{State Correction}. } \]

4.2.6.28 Dimensions of the Main Matrices

Suppose

\[ \mathbf x\in\mathbb R^n, \qquad \mathbf z\in\mathbb R^m, \qquad \mathbf u\in\mathbb R^p. \]

Then:

\[ \mathbf F\in\mathbb R^{n\times n}, \]

\[ \mathbf B\in\mathbb R^{n\times p}, \]

\[ \mathbf H\in\mathbb R^{m\times n}, \]

\[ \mathbf P\in\mathbb R^{n\times n}, \]

\[ \mathbf Q\in\mathbb R^{n\times n}, \]

\[ \mathbf R\in\mathbb R^{m\times m}, \]

\[ \mathbf S\in\mathbb R^{m\times m}, \]

and

\[ \mathbf K\in\mathbb R^{n\times m}. \]

Checking matrix dimensions is one of the simplest and most effective methods for debugging a Kalman Filter implementation.

4.2.6.29 A Simple Position-Velocity Example

Consider

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

For constant velocity,

\[ \mathbf F= \begin{bmatrix} 1&\Delta t\\ 0&1 \end{bmatrix}. \]

Suppose position alone is measured:

\[ z=x+v_m. \]

Then

\[ \mathbf H= \begin{bmatrix} 1&0 \end{bmatrix}. \]

The filter predicts both position and velocity.

The sensor directly measures only position.

Yet the position measurement can also improve velocity estimation because position and velocity errors become correlated through the dynamics.

This demonstrates a fundamental principle:

\[ \boxed{ \text{A Measurement Can Correct States That It Does Not Directly Measure}. } \]

That ability depends on the system dynamics and observability.

4.2.6.30 Cross-Covariance and Indirect State Correction

Suppose

\[ \mathbf P^-= \begin{bmatrix} P_{xx}&P_{xv}\\ P_{vx}&P_{vv} \end{bmatrix} \]

and

\[ H= \begin{bmatrix} 1&0 \end{bmatrix}. \]

Then

\[ K = \frac{1}{P_{xx}+R} \begin{bmatrix} P_{xx}\\ P_{vx} \end{bmatrix}. \]

Notice the second component:

\[ \frac{P_{vx}}{P_{xx}+R}. \]

Even though velocity is not directly measured, the position residual can correct velocity because of the position-velocity cross-covariance.

This is one of the most powerful features of model-based estimation.

4.2.6.31 Kalman Filter as a Bayesian Estimator

The connection to Section 4.2.5 can now be made explicit.

Before measurement:

\[ \boxed{ p(\mathbf x_k|\mathbf z_{1:k-1}) = \mathcal N( \hat{\mathbf x}_k^-, \mathbf P_k^- ) } \]

Measurement likelihood:

\[ \boxed{ p(\mathbf z_k|\mathbf x_k) = \mathcal N( \mathbf H_k\mathbf x_k, \mathbf R_k ) } \]

After measurement:

\[ \boxed{ p(\mathbf x_k|\mathbf z_{1:k}) = \mathcal N( \hat{\mathbf x}_k^+, \mathbf P_k^+ ) } \]

Thus the Kalman Filter performs Bayesian prediction and correction while maintaining only:

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

4.2.6.32 In What Sense Is the Kalman Filter Optimal?

The classical Kalman Filter is optimal under specific assumptions.

For a linear system with appropriate white-noise assumptions, known covariance models, and the standard statistical conditions, the KF provides the minimum mean-square-error estimate.

It is therefore important not to say simply:

The Kalman Filter is always optimal.

A better statement is:

\[ \boxed{ \text{The Kalman Filter is optimal under its assumed linear stochastic model.} } \]

If the model, covariance assumptions, noise statistics, or measurement model are badly incorrect, actual performance can be poor.

4.2.6.33 Core Assumptions of the Classical Kalman Filter

The standard KF formulation assumes a linear process model:

\[ \mathbf x_k = \mathbf F\mathbf x_{k-1} + \mathbf B\mathbf u + \mathbf w. \]

It assumes a linear measurement model:

\[ \mathbf z_k = \mathbf H\mathbf x_k + \mathbf v. \]

Typical assumptions also include:

\[ E[\mathbf w_k]=0, \]

\[ E[\mathbf v_k]=0, \]

with known covariance matrices

\[ Q \]

and

\[ R. \]

Process and measurement noise are commonly assumed mutually uncorrelated in the basic formulation.

Gaussian assumptions are particularly important when interpreting the filter as maintaining the complete posterior probability distribution.

4.2.6.34 What Happens When the System Is Nonlinear?

Real spacecraft systems are frequently nonlinear.

Orbital dynamics may be written

\[ \dot{\mathbf x}=f(\mathbf x,\mathbf u). \]

A camera may measure nonlinear bearing:

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

A LiDAR may measure range:

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

Attitude kinematics using quaternions are also nonlinear.

Therefore the classical linear Kalman Filter cannot directly handle many complete spacecraft navigation problems.

This motivates:

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

\[ \boxed{\text{ESKF}} \]

\[ \boxed{\text{MEKF}} \]

and

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

However, all of them preserve the fundamental prediction–correction structure introduced here.

4.2.6.35 Filter Initialization

Before the first recursion, the estimator needs

\[ \boxed{ \hat{\mathbf x}_0^+ } \]

and

\[ \boxed{ \mathbf P_0^+. } \]

The initial state estimate may come from:

The initial covariance must represent realistic uncertainty.

If P_0 is unrealistically small, the estimator may be overconfident and resist valid measurements.

If P_0 is extremely large, the filter may initially react strongly to measurements and exhibit a longer transient.

Thus initialization affects convergence.

4.2.6.36 Filter Convergence

A Kalman Filter may require time to converge from its initial state and covariance.

During convergence:

Convergence depends on:

A filter cannot reliably estimate an unobservable state merely by running longer.

4.2.6.37 Observability and the Kalman Filter

For a linear time-invariant system,

\[ x_{k+1}=Fx_k \]

and

\[ z_k=Hx_k, \]

the observability matrix is

\[ \boxed{ \mathcal O= \begin{bmatrix} H\\ HF\\ HF^2\\ \vdots\\ HF^{n-1} \end{bmatrix}. } \]

The system is fully observable if

\[ \boxed{ \operatorname{rank}(\mathcal O)=n. } \]

Observability determines whether the available measurements and dynamics contain enough information to reconstruct the state.

Therefore:

\[ \boxed{ \text{Kalman Filter} + \text{Measurements} \not\Rightarrow \text{All States Are Automatically Estimable}. } \]

Sensor geometry and vehicle motion matter.

4.2.6.38 Innovation as a Filter-Health Indicator

For a correctly modelled, consistent filter, innovations should behave approximately according to their predicted statistics.

The innovation sequence is therefore useful for diagnosing:

A persistently non-zero innovation mean may indicate systematic error.

Innovations that are consistently much larger than predicted may indicate an overconfident filter or unmodelled effects.

4.2.6.39 Normalized Innovation Squared

A common consistency statistic is

\[ \boxed{ \text{NIS}_k = \mathbf y_k^T \mathbf S_k^{-1} \mathbf y_k. } \]

Under the standard assumptions, NIS can be compared with bounds derived from a chi-square distribution with degrees of freedom equal to the measurement dimension.

This is useful for:

It provides a normalized measure rather than examining raw residual magnitude alone.

4.2.6.40 Normalized Estimation Error Squared

If truth is available, such as in simulation or Monte Carlo testing, define

\[ \mathbf e_k = \mathbf x_k-\hat{\mathbf x}_k. \]

Then

\[ \boxed{ \text{NEES}_k = \mathbf e_k^T \mathbf P_k^{-1} \mathbf e_k. } \]

NEES evaluates whether actual estimation error is consistent with the covariance reported by the filter.

NIS can be calculated from measurements during operation.

NEES normally requires truth and is therefore especially useful during simulation, Monte Carlo analysis, SIL, HIL, and validation.

4.2.6.41 Filter Accuracy vs Filter Consistency

A filter can produce small state errors but report an incorrect covariance.

Conversely, it may report a covariance consistent with relatively large errors.

Therefore two questions should be evaluated separately:

\[ \boxed{\text{Is the state estimate accurate?}} \]

and

\[ \boxed{\text{Is the stated uncertainty credible?}} \]

Useful performance metrics include:

A flight navigation filter should ideally be both accurate and statistically consistent.

4.2.6.42 Effect of Choosing R Too Small

Suppose the actual sensor is noisy, but the estimator uses an unrealistically small R.

The filter believes the measurement is more accurate than it really is.

Consequences can include:

Conceptually,

\[ \boxed{ R_{\text{assumed}} < R_{\text{actual}} \Rightarrow \text{Overtrust Measurements}. } \]

4.2.6.43 Effect of Choosing R Too Large

If R is too large, the filter under-trusts the sensor.

Consequences may include:

Thus R should represent the actual measurement uncertainty as realistically as possible.

4.2.6.44 Effect of Choosing Q Too Small

If Q is too small, the filter assumes that the dynamic model is extremely accurate.

Consequences may include:

This is particularly dangerous when important dynamics have been omitted.

4.2.6.45 Effect of Choosing Q Too Large

If Q is excessively large, predicted covariance grows quickly.

The filter then tends to rely more strongly on measurements.

Possible consequences include:

Therefore tuning Q and R is a balance between physical modelling and empirical validation.

4.2.6.46 Correlated Measurement Noise

For a multi-dimensional sensor,

\[ R \]

does not necessarily have to be diagonal.

For example,

\[ \mathbf R= \begin{bmatrix} \sigma_x^2&R_{xy}\\ R_{yx}&\sigma_y^2 \end{bmatrix}. \]

Non-zero off-diagonal terms indicate correlated measurement errors.

Ignoring important correlations may cause the filter to count information incorrectly and become inconsistent.

The same concern applies when multiple sensors share common error sources.

4.2.6.47 Sequential Multi-Sensor Updates

Suppose GNSS, camera, and LiDAR measurements arrive at different times.

The filter does not need to wait until all measurements are available simultaneously.

It can perform

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

then a GNSS update,

then another prediction,

then a camera update,

then a LiDAR update whenever available.

This makes recursive filtering naturally suitable for asynchronous spacecraft sensor suites.

4.2.6.48 Multi-Rate Sensor Fusion

Spacecraft sensors commonly operate at different rates.

For example:

The estimator can propagate at a high navigation rate and perform measurement corrections whenever new measurements arrive.

Conceptually,

\[ \boxed{ \text{High-Rate Prediction} + \text{Asynchronous Measurement Updates}. } \]

This is essential in practical GNC systems.

4.2.6.49 Sensor Dropout

If a measurement is unavailable, the corresponding update is skipped.

The estimator continues:

\[ \boxed{ \hat x^+ \rightarrow \text{Prediction} \rightarrow \hat x^-. } \]

Uncertainty generally grows because

\[ Q \]

continues to contribute during propagation.

When the sensor becomes available again, the filter can resume measurement updates.

This allows graceful navigation through temporary sensor outages, provided uncertainty remains within acceptable operational limits.

4.2.6.50 Measurement Gating

Not every available measurement should automatically be accepted.

Suppose an RPO camera produces an incorrect feature match.

If it is blindly processed, it may corrupt the relative-navigation state.

A common architecture is

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

\[ \downarrow \]

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

\[ \downarrow \]

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

\[ \downarrow \]

\[ \boxed{\text{Accept / Reject / Down-Weight}} \]

\[ \downarrow \]

\[ \boxed{\text{Kalman Update}}. \]

NIS or Mahalanobis-distance tests are frequently used for this purpose.

4.2.6.51 Time Synchronization and Latency

The Kalman equations assume that the measurement is associated with the correct state epoch.

In a real spacecraft, sensors may have:

If a measurement generated at t-\tau is incorrectly treated as a measurement at t, the innovation can contain timing-induced error.

This may be particularly serious for fast rotational motion or close-range RPO.

Therefore practical filtering requires:

\[ \boxed{ \text{Measurement Value} + \text{Measurement Covariance} + \text{Correct Timestamp}. } \]

Time synchronization is part of the navigation problem, not merely a software detail.

4.2.6.52 Numerical Implementation Considerations

The textbook equations are mathematically compact, but practical implementation requires numerical care.

Important considerations include:

Rather than explicitly computing

\[ S^{-1}, \]

numerical implementations generally solve the associated linear system using appropriate factorization methods.

Explicit matrix inversion should usually be avoided when a stable linear solve can be used.

4.2.6.53 Covariance Symmetry

The covariance matrix theoretically satisfies

\[ \boxed{ P=P^T. } \]

Finite-precision numerical operations can introduce small asymmetries.

A common numerical safeguard is

\[ \boxed{ P \leftarrow \frac12(P+P^T). } \]

However, repeated loss of symmetry may indicate deeper numerical or implementation problems and should not simply be hidden.

4.2.6.54 Positive-Semidefinite Covariance

A valid covariance matrix should satisfy

\[ \boxed{ \mathbf a^T P \mathbf a \ge0 } \]

for any vector \mathbf a.

Negative covariance eigenvalues caused by numerical errors are physically invalid.

This is another reason why stable covariance-update methods, careful scaling, and numerically robust implementations matter.

For demanding applications, square-root filtering can provide improved numerical properties.

4.2.6.55 Kalman Filter and Coordinate Frames

Every state and measurement must have a clearly defined coordinate frame.

For example:

Suppose a relative-position sensor reports

\[ \mathbf r^S \]

in its sensor frame while the navigation state uses

\[ \mathbf r^H \]

in the Hill frame.

A frame transformation must occur before the measurement model is correctly applied.

Incorrect frame handling can produce filter errors even when the Kalman equations themselves are implemented perfectly.

4.2.6.56 Kalman Filtering for Spacecraft Orbit Navigation

A basic orbital state may be

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

The dynamics propagate the spacecraft trajectory.

Measurements may come from:

The Kalman Filter combines the propagated orbital state with these measurements to continuously correct position and velocity.

For strongly nonlinear orbital dynamics and nonlinear measurements, an EKF or another nonlinear estimator is normally required, but the underlying recursive logic remains the same.

4.2.6.57 Kalman Filtering for Attitude Determination

Attitude estimation may combine:

A common conceptual architecture is

\[ \boxed{\text{Gyroscope}} \rightarrow \boxed{\text{High-Rate Attitude Propagation}} \]

while absolute attitude sensors provide corrections:

\[ \boxed{ \text{Star Tracker / Sun Sensor / Magnetometer} } \rightarrow \boxed{\text{Measurement Update}}. \]

Because quaternion attitude dynamics and vector observations are nonlinear, spacecraft attitude determination commonly uses variants such as the MEKF rather than a direct classical linear KF.

Nevertheless, P, Q, R, innovation, gain, and prediction/correction remain central concepts.

4.2.6.58 Kalman Filtering for IMU/GNSS Navigation

Consider the state

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

The IMU provides high-rate propagation information.

However, integrating gyro and accelerometer measurements causes errors to accumulate because of:

GNSS provides lower-drift absolute position and velocity information.

Thus,

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

and

\[ \boxed{\text{GNSS}} \rightarrow \boxed{\text{Position/Velocity Correction}}. \]

This is a classic complementary sensor-fusion architecture implemented through Kalman-filter principles.

4.2.6.59 Kalman Filtering for RPO

For rendezvous and proximity operations, a relative navigation state might be

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

A more complete state can include relative attitude, rates, biases, and sensor parameters.

The process model may use:

Measurements may come from:

The estimator therefore performs

\[ \boxed{ \text{Relative Dynamics Prediction} + \text{Relative Sensor Updates} \rightarrow \hat{\mathbf x}_{rel}. } \]

4.2.6.60 RPO Measurement Examples

A relative GNSS system may provide

\[ \boxed{ \mathbf z_{GNSS} = \begin{bmatrix} \mathbf r_{rel}\\ \mathbf v_{rel} \end{bmatrix} + \text{noise}. } \]

A camera may provide line-of-sight measurements such as

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

and

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

LiDAR may provide range:

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

Because these optical/range measurement equations are nonlinear, EKF/UKF-type methods may be needed.

But the structure remains:

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

4.2.6.61 Sensor Handover During RPO

Different sensors may dominate at different relative ranges.

A representative architecture may be

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

\[ \downarrow \]

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

\[ \downarrow \]

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

\[ \downarrow \]

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

The estimator should maintain continuity during these transitions.

This means managing:

The Kalman framework is well suited to such sequential updates.

4.2.6.62 State Estimate vs Capture Decision

Suppose capture requires

\[ \|\mathbf r_{rel}\| < r_{capture} \]

and

\[ \|\mathbf v_{rel}\| < v_{capture}. \]

Checking only

\[ \hat{\mathbf r}_{rel} \]

and

\[ \hat{\mathbf v}_{rel} \]

may not be sufficient.

The navigation covariance should also be considered.

For example, an estimated position may lie inside the capture region while its uncertainty extends significantly outside it.

Therefore mission logic can consider both:

\[ \boxed{\text{Estimated Relative State}} \]

and

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

This is one of the most important operational reasons for maintaining covariance.

4.2.6.63 Kalman Filter vs Complementary Filter

A complementary filter combines sensor information using frequency-dependent or fixed-gain relationships.

A Kalman Filter determines weighting through uncertainty propagation.

Conceptually:

\[ \boxed{ \text{Complementary Filter} \rightarrow \text{Frequency/Designed Weighting} } \]

while

\[ \boxed{ \text{Kalman Filter} \rightarrow \text{Model- and Covariance-Based Weighting}. } \]

A complementary filter may be simpler and computationally cheaper.

A Kalman Filter is more systematic when uncertainty, state coupling, bias estimation, and multiple sensor measurements must be handled.

4.2.6.64 Kalman Filter vs Luenberger Observer

A Luenberger observer has the form

\[ \hat x_{k+1} = F\hat x_k + L(z_k-H\hat x_k). \]

The correction structure resembles the Kalman Filter.

The important difference is the gain.

For the observer,

\[ L \]

is designed to achieve desired error dynamics.

For the Kalman Filter,

\[ K \]

is calculated from the estimated uncertainty and assumed stochastic models.

Thus:

\[ \boxed{ \text{Observer Gain} \rightarrow \text{Designed from Error Dynamics} } \]

whereas

\[ \boxed{ \text{Kalman Gain} \rightarrow \text{Derived from Uncertainty}. } \]

4.2.6.65 Steady-State Kalman Filter

For a time-invariant system under suitable conditions, the covariance may converge toward a steady value:

\[ P_k\rightarrow P_\infty. \]

Consequently,

\[ K_k\rightarrow K_\infty. \]

The filter can then behave like a fixed-gain observer.

This provides an important conceptual bridge between observer theory and Kalman filtering.

The steady-state covariance is associated with the discrete algebraic Riccati equation.

4.2.6.66 The Riccati Equation

The covariance recursion underlying the Kalman Filter is a form of the Riccati equation.

For a time-invariant system, repeated prediction and correction may converge toward a steady covariance.

The Riccati equation determines how uncertainty evolves as a balance between:

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

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

and

\[ \boxed{\text{Measurement Information}}. \]

This mathematical structure also appears in optimal-control theory, particularly the Linear Quadratic Regulator.

That connection is one reason estimation and optimal control are often presented together.

4.2.6.67 Duality with LQR

There is a mathematical duality between the Kalman Filter and the Linear Quadratic Regulator.

Very broadly:

The Riccati equation appears in both.

This leads to the classical Linear Quadratic Gaussian (LQG) framework, where:

\[ \boxed{\text{Kalman Filter}} + \boxed{\text{LQR Controller}} \]

form an estimator-controller architecture under appropriate assumptions.

4.2.6.68 Kalman Filter Does Not Remove Sensor Bias Automatically

A common misconception is:

The Kalman Filter removes sensor bias.

It does so only if bias is represented appropriately in the estimation model or its effects are otherwise observable and modelled.

For example, gyro bias may be added to the state:

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

Its evolution may be modelled as

\[ \boxed{ \mathbf b_{g,k+1} = \mathbf b_{g,k} + \mathbf w_{b,k}. } \]

Then measurements that make the bias observable can allow the filter to estimate it.

Therefore:

\[ \boxed{ \text{Unknown Error} + \text{No State/Error Model} \not\Rightarrow \text{Automatic Kalman Correction}. } \]

4.2.6.69 Kalman Filter Does Not Fix a Wrong Model

A sophisticated estimator cannot compensate indefinitely for fundamentally incorrect physics.

Problems may arise from:

The filter may still produce smooth-looking estimates while being wrong.

Therefore estimator development must begin with correct:

\[ \boxed{ \text{Physics} + \text{Frames} + \text{Units} + \text{Timing} + \text{Sensor Models}. } \]

4.2.6.70 Filter Divergence

Filter divergence occurs when the estimated state increasingly departs from the true state or when the covariance no longer represents actual estimation errors.

Possible causes include:

Monitoring innovations and covariance behaviour is therefore essential.

4.2.6.71 Kalman Filter Verification

A filter should not be validated using only one nominal simulation.

A systematic verification campaign should include:

Monte Carlo testing is particularly important.

4.2.6.72 Monte Carlo Validation

For each Monte Carlo run, vary realistic uncertainties and compare

\[ \hat{\mathbf x}(t) \]

with truth

\[ \mathbf x(t). \]

Evaluate:

\[ \boxed{\text{Position Error}} \]

\[ \boxed{\text{Velocity Error}} \]

\[ \boxed{\text{Attitude Error}} \]

\[ \boxed{\text{Bias Error}} \]

as applicable.

Also evaluate covariance consistency using:

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

and

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

The objective is not merely to demonstrate that one trajectory looks smooth.

It is to demonstrate that the estimator performs robustly across the expected uncertainty envelope.

4.2.6.73 Kalman Filter Implementation in Simulink

A conceptual Simulink implementation can be organized into: 


Previous State Estimate
        │
        ▼
State Prediction ───────────────┐
        │                       │
        ▼                       │
Predicted State                 │
                                │
Previous Covariance             │
        │                       │
        ▼                       │
Covariance Prediction           │
        │                       │
        ▼                       │
Predicted Covariance            │
                                │
Sensor Measurement ─────────────┤
                                ▼
                     Measurement Prediction
                                │
                                ▼
                            Innovation
                                │
                                ▼
                     Innovation Covariance
                                │
                                ▼
                           Kalman Gain
                                │
                   ┌────────────┴────────────┐
                   ▼                         ▼
              State Update             Covariance Update
                   │                         │
                   └────────────┬────────────┘
                                ▼
                       Posterior Estimate
                                │
                                └──► Next Time Step

The model should clearly separate:

A critical simulation rule remains:

\[ \boxed{ \text{Estimator Must Receive Sensor Measurements, Not Simulation Truth}. } \]

4.2.6.74 Recommended Software Architecture

A practical implementation can be separated into modules:

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

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

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

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

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

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

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

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

This modular architecture makes testing and debugging easier than placing the entire estimator in one monolithic function.

4.2.6.75 Practical Navigation Pipeline

A realistic spacecraft implementation is therefore more extensive than the textbook KF equations:

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

\[ \downarrow \]

\[ \boxed{\text{Timestamping and Synchronization}} \]

\[ \downarrow \]

\[ \boxed{\text{Calibration / Frame Conversion}} \]

\[ \downarrow \]

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

\[ \downarrow \]

\[ \boxed{\text{State and Covariance Prediction}} \]

\[ \downarrow \]

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

\[ \downarrow \]

\[ \boxed{\text{Kalman Measurement Update}} \]

\[ \downarrow \]

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

\[ \downarrow \]

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

This is the architecture that should be kept in mind when moving from theory to flight software.

4.2.6.76 What the Guidance and Control System Receives

The navigation system should not merely output a raw sensor measurement.

It should provide a coherent estimated state such as

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

It should also maintain uncertainty:

\[ \boxed{\mathbf P}. \]

Guidance then determines desired motion.

Control compares desired and estimated motion.

Therefore:

\[ \boxed{ \text{Sensors} \rightarrow \text{Kalman Filter} \rightarrow \hat x,P \rightarrow \text{Guidance} \rightarrow \text{Control} \rightarrow \text{Actuators}. } \]

4.2.6.77 Key Equations at a Glance

The complete linear Kalman Filter can be summarized by seven principal equations.

4.2.6.77.1 State Prediction

\[ \boxed{ \hat x_k^- = F_{k-1}\hat x_{k-1}^+ + B_{k-1}u_{k-1} } \]

4.2.6.77.2 Covariance Prediction

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

4.2.6.77.3 Innovation

\[ \boxed{ y_k = z_k-H_k\hat x_k^- } \]

4.2.6.77.4 Innovation Covariance

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

4.2.6.77.5 Kalman Gain

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

4.2.6.77.6 State Update

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

4.2.6.77.7 Covariance Update

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

These equations form the core of the classical discrete Kalman Filter.

4.2.6.78 The Entire Kalman Filter in One Conceptual Equation

The essence of the filter can be remembered as

\[ \boxed{ \text{New Estimate} = \text{Model Prediction} + \text{Uncertainty-Weighted Measurement Correction}. } \]

But the complete algorithm actually maintains two parallel recursive processes:

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

and

\[ \boxed{ \text{Uncertainty Evolution}. } \]

These two processes cannot be separated.

Without covariance, the estimator would not know how strongly to trust the measurement.

Without the state estimate, covariance alone has no navigation meaning.

Thus:

\[ \boxed{ \text{Kalman Filter} = \text{State Estimation} + \text{Uncertainty Estimation}. } \]

4.2.6.79 Key Takeaways

The Kalman Filter is a recursive model-based sensor-fusion algorithm for linear stochastic systems.

It combines

\[ \boxed{\text{Dynamic Prediction}} \]

with

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

according to their uncertainties.

Its central quantities are:

\[ \boxed{\hat x,\;P,\;Q,\;R,\;y,\;S,\;K}. \]

The algorithm consists of:

\[ \boxed{ \text{Predict State} \rightarrow \text{Predict Covariance} \rightarrow \text{Predict Measurement} \rightarrow \text{Compute Innovation} \rightarrow \text{Compute Innovation Covariance} \rightarrow \text{Compute Kalman Gain} \rightarrow \text{Correct State} \rightarrow \text{Correct Covariance}. } \]

The process covariance Q describes uncertainty in state evolution.

The measurement covariance R describes sensor uncertainty.

The innovation

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

describes disagreement between prediction and observation.

The innovation covariance

\[ S=HP^-H^T+R \]

determines how significant that disagreement is.

The Kalman gain

\[ K=P^-H^TS^{-1} \]

determines how strongly the measurement changes the state estimate.

A valid measurement generally reduces uncertainty, while propagation without measurements generally increases it.

Measurements can correct states that they do not directly observe through system dynamics and cross-covariance, provided the system is observable.

The filter must be initialized with realistic

\[ \hat x_0 \]

and

\[ P_0. \]

Reliable performance requires physically meaningful Q and R, correct coordinate frames, correct timestamps, correct sensor models, numerical stability, measurement validation, and consistency testing.

Most importantly,

\[ \boxed{ \text{A Kalman Filter Is Not Merely a Noise-Smoothing Algorithm.} } \]

It is a recursive probabilistic state estimator that continuously answers two questions:

\[ \boxed{\text{What is the spacecraft state?}} \]

and

\[ \boxed{\text{How certain are we about that state?}} \]

4.2.6.80 From the Kalman Filter to Nonlinear Spacecraft Estimation

The classical Kalman Filter gives us the fundamental architecture:

\[ \boxed{ \text{Prediction} \rightarrow \text{Innovation} \rightarrow \text{Kalman Gain} \rightarrow \text{Correction}. } \]

However, many important spacecraft equations are nonlinear:

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

\[ z=h(x)+v. \]

Examples include:

Therefore the next challenge is:

How can the Kalman-filter framework be retained when f(\cdot) and h(\cdot) are nonlinear?

The first major solution is to linearize the nonlinear dynamics and measurement models locally around the current estimate.

That leads naturally to the next section:

\[ \boxed{\textbf{4.2.7 Extended Kalman Filter (EKF)}} \]

where the state-transition matrix and measurement matrix are replaced by Jacobians of the nonlinear process and measurement functions, while the prediction–innovation–correction philosophy developed here remains intact.

Next: 4.2.7 Extended Kalman Filter (EKF)

The classical Kalman Filter developed here assumes linear process and measurement models. The next section extends the same prediction–innovation–correction framework to nonlinear spacecraft dynamics and nonlinear sensor models through local Jacobian linearisation.

4.2.7 Extended Kalman Filter (EKF)

Nonlinear process and measurement models, Jacobians, state and covariance propagation, innovation, Kalman correction, spacecraft navigation applications, consistency, limitations, and practical EKF implementation.

Continue to Extended Kalman Filter →