Book contents
Contents
raw Math
RAW Book Control Theory State Estimation

The Kalman Filter

Robert Eisele

We want the state \(x_k\) at time step \(k\), but cannot observe it directly. We estimate it as \(\hat{x}_{k|k}\): the first subscript names the state time, and the index after the vertical bar records the latest observation used. The Kalman filter combines a model prediction with each new measurement and carries both the estimate and its uncertainty forward.

At each time, information arrives in order

At the end of time \(k-1\), the posterior estimate \(\hat{x}_{k-1|k-1}\) and error variance \(P_{k-1|k-1}\) summarize the data so far. In \(\hat{x}_{i|j}\), the first index names the state time and the index after the vertical bar gives the latest observation time used. Thus \(\hat{x}_{k|k-1}\) estimates the current state before the current observation, while \(\hat{x}_{k|k}\) includes it. The sensor supplies \(z_k\), a noisy observation of the latent state \(x_k\); it is not the state itself. Before \(z_k\) arrives, the filter has only the prior prediction.

The Kalman filter's information timeline At each time the true state x is hidden. The estimate and covariance available from past data are propagated into a prior; a noisy sensor observation z then updates them to a posterior. Time k−1Time kTime k+1 known: x̂ₖ₋₁|ₖ₋₁, Pₖ₋₁|ₖ₋₁prior: x̂ₖ|ₖ₋₁, P⁻ₖafter zₖ: x̂ₖ|ₖ, Pₖprior: x̂ₖ₊₁|ₖ, P⁻ₖ₊₁ prediction adds Qnext prediction latent xₖ₋₁latent xₖ (unknown)latent xₖ₊₁ observation zₖ (variance R) gain K applied to the innovation The true state x is hidden at every time; estimates and observations are what the filter has.

At k−1, the posterior summarizes all measurements through the previous time step.

The prior at \(k\) is not a second name for the posterior at \(k-1\): process uncertainty accumulates during propagation. The measurement update happens only when \(z_k\) becomes available.
The hidden quantity and the quantity we can report\(x_k\) is the true state at time \(k\); it exists, but is not directly known. The estimate \(\hat{x}_{k|k-1}\) uses observations only through \(k-1\), while \(\hat{x}_{k|k}\) also uses the measurement at \(k\).

Comparing the two indices distinguishes three tasks. Prediction estimates a state beyond the latest available observation (\(i>j\)). Filtering estimates the state at the observation time (\(i=j\)). Smoothing revises a past state using later observations (\(i<j\)); for example, \(\hat x_{k-1|k}\) is not the previous posterior shown on the timeline.

Discover the correction in a running mean

Suppose the state is constant and we estimate it by averaging measurements. After \(n\) observations, their mean \(\mu_n\) represents a sum of \(n\mu_n\). When one more measurement arrives,

\[\begin{aligned}\mu_{n+1}&=\frac{n\mu_n+z_{n+1}}{n+1}\\&=\mu_n+\frac{1}{n+1}(z_{n+1}-\mu_n).\end{aligned}\]

The mean moves by a fraction of the discrepancy, without revisiting any previous measurement. More generally, an affine combination \((1-K)a+Kb\) of two estimates for the same quantity is \(a+K(b-a)\): its weights sum to one so that agreement leaves the estimate unchanged. The Kalman filter keeps this form but chooses the weight using prediction and measurement uncertainty. In the static case with \(A=1\), no input or process noise, independent measurements of equal variance \(R\), and the diffuse-prior limit, the first measurement gives \(P_1=R\). Thereafter \(P_n=R/n\) and the next gain is exactly \(1/(n+1)\).

What should a single estimate report?

Uncertainty describes a range of possible states, not just one guess. Its mean locates the center and its variance measures the average squared distance from that center, in squared state units. A Gaussian distribution is fully specified by those two numbers, but the following argument needs only finite variance.

Squaring prevents positive and negative errors from canceling and makes large errors count more. If that is the loss we choose, a constant estimate \(a\) has expected loss

\[\begin{aligned}\mathbb E[(X-a)^2]&=\mathbb E[((X-\mu)+(\mu-a))^2]\\&=\operatorname{Var}(X)+(\mu-a)^2,\qquad \mu=\mathbb E[X].\end{aligned}\]

The cross term vanishes because \(\mathbb E[X-\mu]=0\). Only the second term depends on \(a\), so the mean minimizes squared loss. The same argument after observing \(Z\) replaces the mean and variance with their conditional versions: the best estimate is \(\mathbb E[X\mid Z]\). The filtering goal at time \(k\) is therefore \(\mathbb E[x_k\mid z_1,\ldots,z_k]\). In a linear Gaussian model, we can compute it recursively. Other losses can favor other summaries: absolute error, for example, favors a median.

The state model carries knowledge forward

For now, let \(x_k\in\mathbb R\) be one scalar state. A linear model for its evolution is

\[x_k=A_kx_{k-1}+B_ku_k+w_{k-1}.\]

Here \(A_k\) is the state-transition coefficient: it maps the previous state into the part of the next state predicted by the model. In practice, it encodes the system's dynamics, coordinate transformations, and any rescaling needed when the state representation changes between steps. Its units must make each output coordinate consistent with its input coordinates. Sensor calibration belongs in the observation map \(H_k\), while known control inputs \(u_k\) enter through \(B_k\).

The random term \(w_{k-1}\) represents unmodeled changes during the interval; assume it has mean zero and covariance \(Q_{k-1}\), and is independent of the previous estimation error. Process-noise covariance is commonly estimated from model residuals, repeated runs, or physical knowledge of disturbances and uncertain parameters. If a control input itself has centered error \(\eta_k\) with covariance \(U_k\), its state-space contribution is \(B_kU_kB_k^\mathsf T\). Add this to the effective process covariance if the input error is uncorrelated with the other errors and has not already been counted.

We do not know \(x_{k-1}\) exactly. We carry its estimate \(\hat{x}_{k-1|k-1}\) and uncertainty \(P_{k-1|k-1}\). Applying the transition model gives the prior estimate

\[\hat{x}_{k|k-1}=A_k\hat{x}_{k-1|k-1}+B_ku_k.\]

To propagate its uncertainty, define the previous estimation error as \(e_{k-1}=x_{k-1}-\hat{x}_{k-1|k-1}\). The new prior error is \(e_k^-=A_ke_{k-1}+w_{k-1}\). Expanding the variance gives

\[\begin{aligned}P_{k|k-1}&=\operatorname{Var}(A_ke_{k-1}+w_{k-1})\\&=A_k^2P_{k-1|k-1}+2A_k\operatorname{Cov}(e_{k-1},w_{k-1})+Q_{k-1}\\&=A_k^2P_{k-1|k-1}+Q_{k-1}.\end{aligned}\]

The cross term vanishes by the noise assumption, not because prediction and correction happen at different times. Process noise always adds variance relative to propagation without noise. The prior can still be narrower than the previous posterior if the dynamics contract uncertainty sufficiently. With independent Gaussian terms, their sum remains Gaussian, with the displayed mean and variance.

A measurement is another uncertain statement

In the scalar case, suppose the sensor directly observes the current state but adds measurement noise:

\[z_k=x_k+v_k,\qquad \mathbb E[v_k]=0,\qquad \operatorname{Var}(v_k)=R_k.\]

The measurement error \(v_k\) is distinct from process noise \(w_{k-1}\). Process noise describes uncertainty in how the state evolves; measurement noise describes uncertainty in what the sensor reports. In practice, \(R_k\) can come from a sensor data sheet's noise specification adjusted for bandwidth, or from repeated stationary measurements and calibration residuals. Assume the new measurement noise is independent of the prior state error.

Before observing \(z_k\), our state estimate is \(\hat{x}_{k|k-1}\). The difference between the measurement and its predicted value is the innovation,

\[r_k=z_k-\hat{x}_{k|k-1}.\]

It is the new information in the measurement: a large residual says the observation disagrees with the prior prediction, but by itself does not say whether the sensor or the model deserves more trust.

Fuse the prior and the measurement

Consider a linear correction using the innovation: \(\hat{x}_{k|k}=\hat{x}_{k|k-1}+K_kr_k\). The scalar \(K_k\), called the Kalman gain, determines how much of the discrepancy is added to the prediction. Its optimal value minimizes expected squared estimation error.

Let \(e_k^-=x_k-\hat{x}_{k|k-1}\) be the prior error and \(e_k=x_k-\hat{x}_{k|k}\) the posterior error. Since \(z_k=x_k+v_k\), substituting the correction rule gives

\[e_k=(1-K_k)e_k^- - K_kv_k.\]

Before the measurement, \(\mathbb E[e_k^-]=0\) and \(\mathbb E[(e_k^-)^2]=P_{k|k-1}\). The measurement noise has zero mean and variance \(R_k\), and the two errors are independent. Thus the cross term has zero expectation.

Choose the gain that minimizes squared error

The error is random because both the prior error and the sensor noise are random. Its mean square equals its variance here because the errors are centered. With a bias, mean squared error also includes the squared bias.

Squaring the posterior error and taking its expectation therefore produces the scalar objective

\[\begin{aligned}J(K_k)&=\mathbb E[e_k^2]\\&=(1-K_k)^2P_{k|k-1}+K_k^2R_k.\end{aligned}\]

This is a quadratic function of the still-undetermined gain. Its derivative is

\[\frac{dJ}{dK_k}=-2(1-K_k)P_{k|k-1}+2K_kR_k.\]

Setting the derivative to zero gives its unique minimum, because the second derivative is \(2(P_{k|k-1}+R_k)>0\):

\[\boxed{K_k=\frac{P_{k|k-1}}{P_{k|k-1}+R_k}}.\]

If measurement noise \(R_k\) is large, the gain is small and the correction is cautious. If prior uncertainty is large relative to measurement noise, the gain is large and the new observation has more influence. The gain is not a confidence score added after the fact; it is the minimizer of the stated error criterion.

The gain's two limiting casesAs \(R_k\to0\), the sensor becomes exact and \(K_k\to1\): accept the observation. As \(P_{k|k-1}\to0\), the prediction becomes exact and \(K_k\to0\): keep the prior estimate.
Why the remaining error is orthogonal to the innovation

Increasing the gain by \(\delta\) changes the error from \(e_k\) to \(e_k-\delta r_k\), so the change in mean squared error is \(-2\delta\mathbb E[e_kr_k]+\delta^2\mathbb E[r_k^2]\). Unless the mixed term is zero, a small step with the right sign improves the estimate.

\[0=\mathbb E[e_kr_k]=\mathbb E[((1-K_k)e_k^- - K_kv_k)(e_k^-+v_k)]=(1-K_k)P_{k|k-1}-K_kR_k.\]

Solving gives the same gain. No linear correction based on this innovation can improve it further. This is the projection principle behind linear regression and least squares.

The same update also gives posterior uncertainty

The posterior variance is the expected squared posterior error we just minimized. Before substituting the optimal gain,

\[P_{k|k}=(1-K_k)^2P_{k|k-1}+K_k^2R_k.\]

With the optimal gain, this simplifies to

\[\boxed{P_{k|k}=(1-K_k)P_{k|k-1}=\frac{P_{k|k-1}R_k}{P_{k|k-1}+R_k}}.\]

The state estimate and its variance are updated together:

\[\boxed{\hat{x}_{k|k}=\hat{x}_{k|k-1}+K_k(z_k-\hat{x}_{k|k-1}),\qquad P_{k|k}=(1-K_k)P_{k|k-1}.}\]

The corrected estimate lies between the prior estimate and the measurement when both are scalar and their variances are positive. The posterior variance is smaller than either input variance: combining independent information cannot make this particular estimate less precise.

The posterior becomes the next prior

After the update at time \(k\), the pair \((\hat{x}_{k|k},P_{k|k})\) summarizes everything the filter needs from the entire observation history. It becomes the starting knowledge for the next transition. The filter does not need to store or reprocess all earlier measurements.

The recursive Kalman-filter cycleA posterior estimate and variance are propagated through the model to a prior. A new measurement updates them, creating the next posterior.Posterior at k−1estimate + covariancePrior at kpropagate with A, B, QPosterior at kupdate with z, H, Rrepeat when the next time step arrives
Prediction uses the transition model and process uncertainty. Correction uses only the measurement that has just arrived.

If a measurement is missing, there is no innovation or correction: the filter keeps the prior estimate and variance. There is no measurement-driven reduction in uncertainty.

Work through three measurements

Start at time 0 with a position estimate of \(10\,\mathrm m\) and a variance of \(8\,\mathrm m^2\). Take \(A=1\), no known input, \(Q=1\,\mathrm m^2\) per step and \(R=4\,\mathrm m^2\). Before the first measurement the estimate is still 10 and the variance is now 9. The sensor reports 14, giving

\[K_1=\frac9{13},\qquad \hat x_{1|1}=10+\frac9{13}(14-10)=\frac{166}{13},\qquad P_{1|1}=\frac{36}{13}.\]

The result, \(12.77\,\mathrm m\), is closer to the measurement than to the prediction because the measurement variance is smaller. At time 2, the predicted position is still \(166/13\), but the variance grows to \(49/13\). Repeat the same calculation with measurements of 11 and 13 metres:

\[\begin{aligned}K_2&=\frac{49}{101},\qquad P_{2|2}=\frac{196}{101},\\\hat x_{2|2}&=\frac{166}{13}+\frac{49}{101}\left(11-\frac{166}{13}\right)=\frac{1203}{101},\\[.5em]K_3&=\frac{297}{701},\qquad P_{3|3}=\frac{1188}{701},\\\hat x_{3|3}&=\frac{1203}{101}+\frac{297}{701}\left(13-\frac{1203}{101}\right)=\frac{8673}{701}.\end{aligned}\]

Before the third measurement, the variance is \(196/101+1=297/101\); the estimates at times 2 and 3 are about 11.91 and 12.37 metres. Each row moves from the previous posterior to the new prior before using the measurement, just as on the timeline.

Prediction adds variance; correction adds precision

Prediction adds a new random error to the propagated old error. Correction does not add random variables: it combines two statements about the same unknown position. For positive prior variance \(P^-\) and independent Gaussian measurement noise of variance \(R\), Bayes' rule gives

\[p(x\mid z)\propto \exp\left(-\frac{(x-\hat x^-)^2}{2P^-}\right)\exp\left(-\frac{(z-x)^2}{2R}\right).\]

The negative log of the product is quadratic in \(x\). Its quadratic coefficient is \(\tfrac12(1/P^-+1/R)\); its linear coefficient is \(-(\hat x^-/P^-+z/R)\). Completing the square gives a Gaussian with

\[\frac1{P^+}=\frac1{P^-}+\frac1R,\qquad \hat x^+=P^+\left(\frac{\hat x^-}{P^-}+\frac zR\right)=\hat x^-+\frac{P^-}{P^-+R}(z-\hat x^-).\]

Precision is inverse variance: the steeper the quadratic, the narrower the distribution. Here the prior and the likelihood contribute separate curvatures, and the result recovers both the gain and the posterior variance from the squared-error derivation.

previous posteriorprior
Prediction at fixed position: a Gaussian process error of variance \(Q=P^-/2\) widens the previous posterior to \(P^-\).
priorlikelihoodposterior
Update: the prior is centered at 10 m, the measurement remains at 14 m, and only their uncertainties change.

Gain 0.692 · Estimate 12.77 m · Posterior variance 2.77 m²

When does the filter trust the model?

For a scalar sensor observing the state directly, raising \(R\) at fixed \(P^-\) reduces the gain; raising \(Q\) increases the next prior variance and thus gives the next measurement more weight, all else equal. With \(A=1\), \(Q=0\), and equally precise measurements, the gain keeps falling as evidence accumulates. With \(Q>0\), the filter allows for continuing changes and retains sensitivity to new observations. In the scalar steady-state case with \(A=1\) and constant gain, the estimate behaves like a first-order low-pass filter; the general Kalman filter is a dynamic state estimator, not a fixed smoothing rule.

Follow the information through a generic scalar filter

This simulator uses the scalar model \(x_k=A x_{k-1}+w_{k-1}\), with measurements \(z_k=x_k+v_k\). The latent path is shown only because this is a simulation; in an actual estimation problem the true state is unavailable. Change the process and measurement variances, or omit periodic observations, and watch the same recursion respond.

Scalar recursion

latent state (simulation only)sensor measurementsfiltered estimate±2 SD interval
The complete sequence is always visible. Changing A, Q, or R regenerates and refilters the same seeded sequence; missing observations leave the prediction uncorrected.
Final time step32
Final gain Kk—
Prior variance P−k—
Posterior variance Pk—

The full 32-step sequence is shown. Variances and gains update when model or noise parameters change.

Why the state becomes a vector

If evolution depends on several hidden quantities, collect them into a state vector \(\mathbf x_k\in\mathbb R^n\). In the Gaussian model its estimate \(\hat{\mathbf x}_{k|k}\) is the conditional mean. The diagonal entries of \(P_{k|k}\) are error variances; the off-diagonal entries are error covariances.

The vector state model is \(\mathbf x_k=A_k\mathbf x_{k-1}+B_k\mathbf u_k+\mathbf w_{k-1}\). If \(\mathbf x_k\) has coordinates \(x_k^{(1)},\ldots,x_k^{(n)}\), then entry \((A_k)_{ij}\) specifies how much old coordinate \(x_{k-1}^{(j)}\) contributes to predicted coordinate \(x_k^{(i)}\). Each row forms one predicted coordinate; off-diagonal entries couple coordinates. The matrix \(B_k\) maps known controls \(\mathbf u_k\) into state changes, while the process-noise vector \(\mathbf w_{k-1}\), with covariance \(Q_{k-1}\), accounts for unmodeled change. Together they give the prior

\[\hat{\mathbf x}_{k|k-1}=A_k\hat{\mathbf x}_{k-1|k-1}+B_k\mathbf u_k,\qquad P_{k|k-1}=A_kP_{k-1|k-1}A_k^\mathsf T+Q_{k-1}.\]

A sensor need not reveal the entire state. Its matrix \(H_k\) maps the state into the measured coordinates: \(\mathbf z_k=H_k\mathbf x_k+\mathbf v_k\), where \(\mathbf v_k\) has measurement covariance \(R_k\). Thus a sensor can observe a projection of the hidden vector, while correlations in \(P\) allow that partial information to affect other state coordinates.

How can a position measurement change an estimated velocity?

For a constant-velocity model, \(\Delta t\) seconds of motion turns velocity into a position increment. A position sensor selects just the first coordinate:

\[\mathbf x_k=\begin{pmatrix}p_k\\v_k\end{pmatrix},\qquad A=\begin{pmatrix}1&\Delta t\\0&1\end{pmatrix},\qquad H=\begin{pmatrix}1&0\end{pmatrix}.\]

Position and velocity errors can be correlated even though the sensor only observes position. For instance, underestimating velocity can pull a later predicted position too low. Suppose the prior covariance and sensor variance are

\[P^-=\begin{pmatrix}9&3\\3&4\end{pmatrix},\qquad R=4.\]

The position residual contains position error plus sensor noise, so it has variance \(9+4=13\). It has covariance 9 with the position error and 3 with the velocity error. The same projection principle as in the scalar case divides these covariances by the residual variance. Thus a single residual \(r\) moves the state by \((9r/13,\,3r/13)^\mathsf T\): at \(r=4\,\mathrm m\), the velocity estimate increases by \(12/13\,\mathrm{m/s}\).

priorposition measurementposterior
Each ellipse is a two-standard-deviation contour in its principal directions. A positive or negative tilt reflects how the errors vary together. At zero correlation, the position update leaves the velocity mean and variance unchanged.

Position 12.77 m · Velocity 0.92 m/s

The matrix update from Gaussian conditioning

Before the measurement, suppose \(\mathbf x_k\) has Gaussian prior \(\mathcal N(\hat{\mathbf x}^-_k,P^-_k)\), and the independent measurement noise is Gaussian with covariance \(R_k\). The innovation is \(\mathbf r_k=\mathbf z_k-H_k\hat{\mathbf x}^-_k\). Because it is a linear function of Gaussian variables, it is Gaussian with covariance

\[S_k=H_kP^-_kH_k^\mathsf T+R_k.\]

The state error and innovation have cross-covariance \(\operatorname{Cov}(\mathbf x_k-\hat{\mathbf x}^-_k,\mathbf r_k)=P^-_kH_k^\mathsf T\). Conditioning one jointly Gaussian vector on the other therefore gives

\[K_k=P^-_kH_k^\mathsf T S_k^{-1},\qquad \hat{\mathbf x}_{k|k}=\hat{\mathbf x}^-_k+K_k\mathbf r_k.\]

The matrix gain is the multidimensional version of the scalar weight. It maps measurement-space residuals into state-space corrections; its entries need not lie between zero and one and can carry units.

Derive the matrix update from prior and likelihood

Bayes' rule says \(p(\mathbf x_k\mid\mathbf z_k)\propto p(\mathbf z_k\mid\mathbf x_k)p(\mathbf x_k)\). The prior is \(\mathcal N(\hat{\mathbf x}^-_k,P^-_k)\), and the likelihood is centered on the predicted measurement \(H_k\mathbf x_k\):

\[\mathbf z_k\mid\mathbf x_k\sim\mathcal N(H_k\mathbf x_k,R_k).\]

For invertible prior and noise covariances, collecting the quadratic and linear terms in \(\mathbf x_k\) gives posterior precision and mean:

\[\begin{aligned}P_{k|k}^{-1}&=(P^-_k)^{-1}+H_k^\mathsf T R_k^{-1}H_k,\\\hat{\mathbf x}_{k|k}&=P_{k|k}\left((P^-_k)^{-1}\hat{\mathbf x}^-_k+H_k^\mathsf T R_k^{-1}\mathbf z_k\right).\end{aligned}\]

Applying the matrix inversion lemma to this precision form yields the innovation covariance \(S_k\) and gain \(K_k=P^-_kH_k^\mathsf TS_k^{-1}\) above. The matrix formula is therefore the same Bayesian fusion as the scalar derivation, expressed for coupled state coordinates.

What “optimal” means

In a linear Gaussian model, the filter computes the exact conditional mean and covariance, so it minimizes mean squared error (MMSE) among all estimators based on the observations. Without Gaussianity, the recursion still gives the best affine estimator (LMMSE) under the usual second-moment assumptions: the initial error and the centered process and measurement noises are mutually uncorrelated across terms and times, with the modeled covariances. Its error covariance need not then equal the conditional covariance for a particular observation history.

Deterministic weighted least squares instead chooses values that minimize a weighted sum of squared residuals. In a Gaussian model the same weighted quadratic is a negative log density, which explains the algebraic connection; a deterministic least-squares objective alone does not establish a probabilistic MMSE guarantee.

The scalar derivation made this precise: the gain minimizes \(\mathbb E[e_k^2]\). The matrix gain is the corresponding covariance-weighted projection. No one gain is optimal independently of a loss function and model assumptions.

When the model is nonlinear

Many physical systems have nonlinear transitions \(\mathbf x_k=f(\mathbf x_{k-1},\mathbf u_k)+\mathbf w_{k-1}\) or nonlinear measurements \(\mathbf z_k=h(\mathbf x_k)+\mathbf v_k\). The ordinary Kalman filter no longer gives an exact Gaussian posterior in general. The extended Kalman filter linearizes \(f\) and \(h\) around the current estimate; the unscented Kalman filter instead propagates a chosen set of sigma points. Both are approximations, and neither removes the need to choose a model and noise covariances carefully.

If observations arrive irregularly, discretize both the transition and process noise for the actual elapsed time (and the input map when appropriate). If a sensor fails to report, perform prediction only and wait for the next available measurement.

A small covariance is not proof of truth. The filter is only as credible as its transition model, measurement model, and covariance assumptions. An underestimated \(Q_k\) or \(R_k\), unmodeled bias, or correlated noise can make the estimate confidently wrong.

Implementation and numerical stability

In the correction, the new error is \((I-K_kH_k)\mathbf e_k^- - K_k\mathbf v_k\). Since the prior error and measurement noise are uncorrelated, their transformed covariances add. This gives the Joseph form:

\[P_{k|k}=(I-K_kH_k)P^-_k(I-K_kH_k)^\mathsf T+K_kR_kK_k^\mathsf T.\]

For the optimal gain in exact arithmetic it simplifies to \((I-K_kH_k)P^-_k\). The Joseph form holds for any fixed gain under the stated noise assumption; the shortcut does not. Its sum of symmetric positive semidefinite terms better preserves that structure in finite precision.

At every time step, first propagate the previous posterior. Then, if a measurement is available, correct the prior:

# Predict from time k-1 to time k
x_prior = A @ x_posterior + B @ u
P_prior = A @ P_posterior @ A.T + Q

# Correct only when z_k is available
x_posterior, P_posterior = x_prior, P_prior
if z is available:
  S = H @ P_prior @ H.T + R
  K = solve(S, H @ P_prior).T
  x_posterior = x_prior + K @ (z - H @ x_prior)
  M = I - K @ H
  P_posterior = M @ P_prior @ M.T + K @ R @ K.T

Here solve(S, B) returns the solution of \(SX=B\), without forming \(S^{-1}\). For positive-definite innovation covariance, a Cholesky factorization is a standard stable choice.

References

  • [Kalman1960]Kalman, R. E. (1960). A New Approach to Linear Filtering and Prediction Problems. Journal of Basic Engineering, 82(1), 35–45.
  • [Rojas]Rojas, R. The Kalman Filter.
  • [Ribeiro]Ribeiro, M. I. Introduction to Kalman Filtering.
  • [WelchBishop]Welch, G., and Bishop, G. An Introduction to the Kalman Filter.
  • [Shimkin]Shimkin, N. Derivations of the Discrete-Time Kalman Filter.
  • [Koks]Koks, D. Proofs and Techniques Useful for Deriving the Kalman Filter.
  • [Bell]Bell, B. M. The Kalman Filter and Smoother.