# Kalman Filter Design

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?

• Given that $\Phi = \mathbf{I}$, just letting $\hat{ \mathbf{X}}$ equal a long average of your measured $\mathbf{Z}_n$ times $\mathbf{H}^{-1}$ would work fine, and you wouldn't have a lot of egregiously complicated math to go wrong. It'd take a bit longer to calibrate, but in a production environment, if you can do a process by whacking something with a rock, you shouldn't make a lace doily out of titanium to do the job -- you should show the manufacturing staff how to whack the thing with a rock! – TimWescott Nov 3 '20 at 16:18

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!

• Nitpicky, so I'm not going to edit your nice answer: "such that you get an estimate of the output when you multiply $H$ by your state vector $X$". To be really precise, you'd end that with "by your estimated state vector $X$". Then maybe really drive the point home by calling it $\hat X$ or something. – TimWescott Nov 3 '20 at 16:04
• @TimWescott - Well, it works either way, right? If you had the actual state $X$ then $H$ should give you the actual measurement (less noise). If you had a state estimate $\hat{X}$, then $H$ would give you a measurement estimate, right? But it's a fair point so I'll edit the answer myself :) – Chuck Nov 3 '20 at 16:08
• @TimWescott - Edited the answer, but I left the equation alone because it's taken verbatim from the text OP linked. – Chuck Nov 3 '20 at 16:10
• @Chuck thank you again for taking the time to answer my question! The excellent explanation above makes it much clearer to me. However, I'm now confused by the example in the paper as $$Z_{k} = H_{k} X_{k} = \begin{bmatrix} b + rP_{1} \\ b - rP_{1} \\ b + \frac{r}{\sqrt{2}}P_{1} + \frac{r}{\sqrt{2}}P_{2} \\ b + \frac{r}{\sqrt{2}}P_{1} + \frac{r}{\sqrt{2}}P_{3} \end{bmatrix}$$ would this then imply that you need 4 measurements? – Joe Nov 3 '20 at 23:35
• @Joe - sorry for not getting back to you earlier. Really it should be H that determines how many measurements you have, and H combines/modifies your states appropriately. It looks kind of like your states are rotation and position for a center of mass but your measurements are maybe rotation and position as measured at some point that isn't the center of mass. I can't tell if your equation is correct or not but that's what it looks like to me. – Chuck Nov 8 '20 at 14:42