control//state estimation//state-space model
How a system is written so that a filter can predict it and compare it with its sensors. The **state** \(x\) is a vector holding the minimum information needed to predict the system's future, together with the known inputs (sufficient state). A car on a straight road needs \([p,v]^{\mathsf T}\), position and velocity; a drone needs position, velocity, orientation and angular velocity, twelve numbers or more, and real filters append the biases of their sensors. Two equations describe it:
How a system is written so that a filter can predict it and compare it with its sensors. The state xxx is a vector holding the minimum information needed to predict the system's future, together with the known inputs (sufficient state). A car on a straight road needs [p,v]T[p,v]^{\mathsf T}[p,v]T, position and velocity; a drone needs position, velocity, orientation and angular velocity, twelve numbers or more, and real filters append the biases of their sensors. Two equations describe it:
xk=F xk−1+wk−1,zk=H xk+vk.x_k=F\,x_{k-1}+w_{k-1},\qquad z_k=H\,x_k+v_k.xk=Fxk−1+wk−1,zk=Hxk+vk.
The first is how the world evolves according to you, with www the process noise. The second is what the sensor would read if the state were known, with vvv the measurement noise.
(F) is the physics. Constant velocity reads pk=pk−1+vk−1Δtp_k=p_{k-1}+v_{k-1}\Delta tpk=pk−1+vk−1Δt, vk=vk−1v_k=v_{k-1}vk=vk−1, which is F=[1Δt01]F=\begin{bmatrix}1&\Delta t\0&1\end{bmatrix}F=[10Δt1]. Velocity is in the state and acceleration is not because this model does not need it; a car that brakes and accelerates hard is predicted better with aaa in the state, at the cost of more to estimate and more that may not be observable.
(H) is the translator. The state lives in its own space (metres and metres per second), the sensor in another (a GPS gives only a position), and pears cannot be subtracted from apples. Hx^−H\hat x^-Hx^− answers what the sensor should read if my prediction were true: H=[1 0]H=[1;0]H=[10] for a GPS, [0 1][0;1][01] for a wheel encoder, the identity when both are measured, and for a thermocouple that outputs millivolts, a gain of about 0.041 mV/°C. HHH selects, combines and converts units, which is why it multiplies the long vector: it projects it into the sensor's language.
Dimensions check every equation. With nnn states and mmm measured quantities, x^\hat xx^ is n×1n\times1n×1; PPP, FFF and QQQ are n×nn\times nn×n; zzz and the innovation m×1m\times1m×1; HHH is m×nm\times nm×n; RRR and SSS are m×mm\times mm×m; the gain is n×mn\times mn×m, because it turns a disagreement in the sensor's language into a correction in the state's. If the dimensions do not fit, the equation is wrong.
How the uncertainty moves through FFF and HHH is covariance propagation.
?QuestionIn code, do I compute S−1S^{-1}S−1 with np.linalg.inv?
Better not. Inverting explicitly is slower and numerically less stable; solve the system instead, K = np.linalg.solve(S, H @ P_pred).T, which uses the fact that SSS and PPP are symmetric. With a few measured quantities it makes no difference; with many sensors, it does.