State Estimation

Revise

Multivariate Kalman Filter

Note

May be difficult to grasp due to the sheer number in matrices along with understanding covariance. Do more review on Linear Algebra and Probability and Statistics then revisit later to solidify understanding.

Unlike the one-dimensional Kalman Filter, the Multivariate Kalman Filter works in multiple dimensions.

Common practice to describe a multidimensional process with a single equation in matrix form.

Example: Nine-dimensional state vector that describes position, velocity, and acceleration.

$$\left[\begin{array}{c}x\\ y\\ z\\ \dot{x}\\ \dot{y}\\ \dot{z}\\ \ddot{x}\\ \ddot{y}\\ \ddot{z}\end{array}\right]$$

MKF outputs a multivariate random variable in which a covariance matrix describes the squared uncertainty of the multivariate random variable.

How It Works

After initialization, the MKF predicts the system state at the next step and provides the uncertainty of the prediction.

Once the measurement is received, it will correct i.e. update the prediction along with the uncertainty of the current state.

Rinse and repeat.

Kalman Filter Equations for Multivariate

State Predict

$${\hat{x}}_{n+1,n}=\mathbf{F}{\hat{x}}_{n,n}+\mathbf{G}{u}_n+w_n$$

Predict Covariance

$${\mathbf{P}}_{n+1,n}=\mathbf{F}{\mathbf{P}}_{n,n}{\mathbf{F}}^{\mathbf{T}}$$

Measurement

$$\overrightarrow{z_n}=\mathbf{H}{x}_n+\overrightarrow{v_n}$$

The Observation Matrix

Often, the measured value is not the desired system state. Therefore, the purpose of observation matrix $\mathbf{H}$ is to convert the system state into outputs using linear transformations.

State Selection

• Sometimes certain states are measured while others are not. In the example provided below, the first, third, and fifth states of the five-dimensional vector are measurable, while second and fourth are not.

$$z_n=\mathbf{H}{x}_n+v_n$$ $$z_n=\left[\begin{array}{ccccc}1& 0& 0& 0& 0\\ 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_1\\ x_2\\ x_3\\ x_4\\ x_5\end{array}\right]+v_n$$ $$z_n=\left[\begin{array}{c}{x}_1\\ x_2\\ x_3\end{array}\right]+v_n$$

Related to selecting states in the configuration for Perfecting Odometry?

State Update

$${\hat{x}}_{n,n}={\hat{x}}_{n,n-1}+K_n(z_n-\mathbf{H}{\hat{x}}_{n,n-1})$$

Note

If, for instance, the state vector has 5 dimensions, while only 3 dimensions are measurable:

$$x_n=\left[\begin{array}{c}{x}_1\\ x_2\\ x_3\\ x_4\\ x_5\end{array}\right]z_n=\left[\begin{array}{c}{z}_1\\ z_3\\ z_5\end{array}\right]$$

Observation matrix $\mathbf{H}$ would be a $3\times 5$ matrix.

$$H=\left[\begin{array}{ccccc}1& 0& 0& 0& 0\\ 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1\end{array}\right]$$

Therefore, the innovation yields:

$$(z_n-\mathbf{H}{\hat{x}}_{n,n-1})=\left[\begin{array}{c}{z}_1\\ z_3\\ z_5\end{array}\right]-\left[\begin{array}{ccccc}1& 0& 0& 0& 0\\ 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_1\\ x_2\\ x_3\\ x_4\\ x_5\end{array}\right]=\left[\begin{array}{c}(z_1-{\hat{x}}_1)\\ (z_3-{\hat{x}}_3)\\ (z_5-{\hat{x}}_5)\end{array}\right]$$

Covariance Update

$${\mathbf{P}}_{n,n}=(\mathbf{I}-{\mathbf{K}}_n\mathbf{H}){\mathbf{P}}_{n,n-1}(\mathbf{I}-{\mathbf{K}}_n\mathbf{H}{)}^T+{\mathbf{K}}_n{\mathbf{R}}_n{\mathbf{K}}_n^T$$

• $\mathbf{I}$ is the Identity Matrix

Kalman Gain

$${\mathbf{K}}_n={\mathbf{P}}_{n,n-1}{\mathbf{H}}^T(\mathbf{H}{\mathbf{P}}_{n,n-1}{\mathbf{H}}^T+{\mathbf{R}}_n{)}^{-1}$$

Notation Table

• $n_x$ is the number of states in a state vector.
• $n_z$ is the number of measured states.
• $n_u$ is the number of elements of the input variable.

References

KFGU Section 2