Kalman Filter Design

Question

I'm new to Kalman filter design and I'm struggling to understand how to apply the Kalman filter methodology to my problem. I've read a research paper which seems to describe what I'm trying to do https://journals.sagepub.com/doi/pdf/10.5772/57516. The paper describes experiments in which the data collected consists of a noisy measurement paired with a truth measurement. In the paper the Kalman filter matrices are designated as follows:

$$\boldsymbol{X}=\begin{bmatrix} b_{x} &P_{11} &P_{12} &P_{13} \end{bmatrix}^{T}$$

$$\boldsymbol{H}=\begin{bmatrix} 1 &r &0 &0\\ 1 &-r &0 &0\\ 1 &r/\sqrt{2} &r/\sqrt{2} &0\\ 1 &r/\sqrt{2} &0 &r/\sqrt{2}\\ \end{bmatrix}$$

$$\boldsymbol{\Phi} = \boldsymbol{I}_{4\times4}$$

The state vector describes bias and scale factor states. Whilst I'm not entirely sure how the H matrix was derived I can't understand how the filter would be used with real data i.e. the observed measurement replaces the elements r in the H matrix, but the ground truth which this is to be calibrated or compared against is a single value i.e. we know the true rate or true acceleration, we do not know the true bias and scale factor states. How then does the filter include this ground truth which I assume it uses to update its estimate of the bias and scale factor terms. Could someone shed some light on how the Kalman filter is laid out to allow the estimation to use this pairing of measurement and ground truth?

Answer 1

The measurements don't get inserted into $H$. The $H$ matrix is the "measurement matrix" or "output matrix" such that you get an estimate of the output when you multiply $H$ by your state vector estimate $\hat{X}$. You can see this in equation (23) in the paper you linked, on document page 7:

The model can be expressed as follows:

$$ X_{i+1} = \Phi X_i + w_i \\ Z_{i+1} = HX_{i+1} + \epsilon_{i+1} \\ $$

The Kalman filter action happens in equation (25), when the Kalman gain $K_i$ is applied to the measurement error or "residual." The measurement error is the difference between what you actually saw, the measurement vector $Z_i$, and what you expected to see, which is the output matrix $H_i$ times the predicted state $\hat{X}_{i|i-1}$.

The Kalman filter takes that modified residual and adds it to the predicted state to get the "corrected" or filtered state. It all happens as one equation in (25):

$$ \hat{X}_i = \hat{X}_{i|i-1} + K_i\left(Z_i - H_i\hat{X}_{i|i-1}\right) \\ $$

Where again the $\hat{X}_{i|i-1}$ term means, "given my previous state $i-1$, what does the model say my state at $i$ should be?" Rewriting it from the earlier equation:

$$ X_{i+1} = \Phi X_i + w_i \\ $$

or

$$ X_{i|i-1} = \Phi X_{i-1} \\ $$

or

$$ X_{i|i-1} = (I + A\Delta t)X_{i-1} + B\Delta t u \\ $$

which is just a numeric integration of

$$ \dot{x} = Ax + Bu \\ $$

such that

$$ x_i = x_{i-1} + \dot{x} \Delta t\\ x_i = x_{i-1} + (Ax_{i-1} + Bu)\Delta t \\ x_i = x_{i-1} + A\Delta t x_{i-1} + B \Delta t u\\ x_i = (I + A\Delta t) x_{i-1} + B\Delta t u \\ $$

Hope this makes more sense!