/ electronics / dsp /

[edit]

Definition

The Kalman Filter is a linear quadratic estimator used to estimate the state of a system by combining sensor measurements and a (physical) process model of the system. It works optimal if measurements and process variations have white gaussian noise.

Working Principle

The Kalman Filter averages a prediction of a system’s state with a new measurement using a weighted average. The purpose of the weights is that estimated values with smaller uncertainty are “trusted” more. The result of the weighted average is a new state estimate that lies between the predicted and measured state, and has a smaller estimated uncertainty than either alone.

The Kalman filter works in two steps: predict and update.

Defining the Model

State Space

The dynamic model for the physical process is

𝐱n+1=𝐆n𝐱n+𝐁𝐮n+𝐰n𝐲n=𝐇n𝐱n+𝐯n\begin{array}{ll} \boldsymbol x_{n+1} & = \boldsymbol{G}_n \boldsymbol x_{n} + \boldsymbol{B} \boldsymbol u_n + \boldsymbol w_n \\ \boldsymbol y_{n} & = \boldsymbol{H}_{n} \boldsymbol x_{n} + \boldsymbol v_{n} \end{array}

with the kk states 𝐱\boldsymbol x, transition matrix 𝐆\boldsymbol{G}, gaussian process noise 𝐰n\boldsymbol w_n, input 𝐮\boldsymbol u, ll measurements 𝐲\boldsymbol y, measurement model 𝐇\boldsymbol{H}, gaussian measurement noise 𝐯n\boldsymbol v_n, time point nn.

The measurement matrix 𝐇\boldsymbol{H} defines how the observations correspond to the state. If the state variables can be directly observed then 𝐇=1\boldsymbol{H} = \boldsymbol{1}.

If there are no known inputs to the process, then 𝐁𝐮n=0\boldsymbol{B} \boldsymbol u_n = 0 and this term can be removed.

Noise

The overall uncertainty of the estimation is expressed with the covariance matrix 𝐏n\boldsymbol{P}_n. This matrix is influenced by the process noise 𝐰n\boldsymbol w_n and the measurement noise 𝐯n\boldsymbol v_n. During prediction, the process noise increases the uncertainty, whereas combining it with the measurement during the update phase decreases the uncertainty.

𝐰n𝒩(0,𝐐n)𝐯n𝒩(0,𝐑n)\begin{array}{ll} \boldsymbol w_n & \sim \mathcal{N}(\boldsymbol{0}, \boldsymbol{Q}_n) \\ \boldsymbol v_n & \sim \mathcal{N}(\boldsymbol{0}, \boldsymbol{R}_n) \end{array}

If values of 𝐐\boldsymbol{Q} are larger than values of 𝐑\boldsymbol{R}, the filter trusts more the process, less the measurements.

Calculations

1. Step: Prediction

1.1 calculate the new (a priori) state estimate based on the old state and the dynamic model (e.g. physical laws)

𝐱̂n|n1=𝐆n𝐱̂n1|n1\hat {\boldsymbol x}_{n|n-1} = \boldsymbol{G}_n \hat{\boldsymbol x}_{n-1|n-1}

1.2 calculate a new process covariance (how certain is the model?)

𝐏𝐱n|n1=𝐆n𝐏𝐱n1|n1𝐆n+𝐐n\boldsymbol{P}_{\boldsymbol x_{n|n-1}} = \boldsymbol{G}_n \boldsymbol{P}_{\boldsymbol x_{n-1|n-1}} \boldsymbol{G}_n^\top + \boldsymbol{Q}_n

2. Step: Update

2.1 calculate intermediate values (optional):

2.2 calculate optimal Kalman-gain:

𝐊n=𝐏𝐱n|n1𝐇n𝐇n𝐏𝐱n|n1𝐇n+𝐑n=𝐏𝐱n|n1𝐇n𝐒1\boldsymbol{K}_n = \frac{\boldsymbol{P}_{\boldsymbol x_{n|n-1}} \boldsymbol{H}_{n}^\top}{\boldsymbol{H}_{n} \boldsymbol{P}_{\boldsymbol x_{n|n-1}} \boldsymbol{H}_{n}^\top + \boldsymbol{R}_n} = \boldsymbol{P}_{\boldsymbol x_{n|n-1}} \boldsymbol{H}_{n}^\top {\boldsymbol{S}}^{-1}

2.3 calculate the updated (a posteriori) state estimate using ll measurements:

𝐱̂n|n=𝐱̂n|n1+𝐊nΔ𝐲n\hat{\boldsymbol x}_{n|n} = \hat{\boldsymbol x}_{n|n-1} + \boldsymbol{K}_n \Delta \boldsymbol y_n which is the estimation of 𝐱n\boldsymbol x_n based on Δyn\Delta y_n with Kij[0.0;1.0]K_{ij} \in [0.0; 1.0] where 0.0 means the filter fully trusts the prediction and 1.0 means the filter fully trusts the measurement.

2.4 update process covariance:

𝐏𝐱n|n=𝐏𝐱n|n1+𝐊n𝐇n𝐏𝐱n|n1\boldsymbol{P}_{\boldsymbol x_{n|n}} = \boldsymbol{P}_{\boldsymbol x_{n|n-1}} + \boldsymbol{K}_n \boldsymbol{H}_{n} \boldsymbol{P}_{\boldsymbol x_{n|n-1}}

Extended Kalman Filter

The Extended Kalman Filter (EKF) uses non-linear dynamic models.

𝐱n=g(𝐱n1,𝐮n)+𝐰n𝐲n=h(𝐱n1)+𝐯n\begin{array}{ll} \boldsymbol x_{n} & = g(\boldsymbol x_{n-1}, \boldsymbol u_n) + \boldsymbol w_n \\[0.5em] \boldsymbol y_{n} & = h(\boldsymbol x_{n-1}) + \boldsymbol v_{n} \end{array} where g()g() and h()h() are non-linear functions. For covariance the Jacobi-Matrix of the model is used:

𝐆=gx|𝐱̂n1,𝐮n𝐇=hx|𝐱̂n\boldsymbol{G} = \frac{\partial g}{\partial x} \Big\vert_{\hat{\boldsymbol x}_{n-1}, \boldsymbol u_n} \qquad \boldsymbol{H} = \frac{\partial h}{\partial x} \Big\vert_{\hat{\boldsymbol x}_{n}}

Sensor Fusion

If ii sensors measure the same state xjx_j, this can be expressed in the measurement matrix 𝐇\boldsymbol{H}. The column jj will have ii rows with entries.

Example

Example

Kalman Filter for Gyroscope and Accelerometer:

state 𝐱\boldsymbol x are the orientation angles roll and pitch and the bias angle

References