# kalman filter with redundant sensors

Suppose I have one robot with two 3D position sensors based on different physical principles and I want to run them through a Kalman filter. I construct an observation matrix two represent my two sensors by vertically concatenating two identity matrices.

$H = \begin{bmatrix} 1&0&0\\0&1&0\\0&0&1\\1&0&0\\0&1&0\\0&0&1 \end{bmatrix}$ $\hspace{20pt}$ $\overrightarrow x = \begin{bmatrix} x\\y\\z \end{bmatrix}$

so that

$H \overrightarrow x = \begin{bmatrix} x\\y\\z\\x\\y\\z \end{bmatrix}$

which represents both sensors reading the exact position of the robot. Makes sense so far. The problem comes when I compute the innovation covariance

$S_k = R + HP_{k|k-1}H^T$

Since

$H H^T = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 1 \\ 0 & 1 & 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 & 1 & 0 \\ 0 & 1 & 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 & 1 & 0 \\ 1 & 0 & 0 & 0 & 0 & 1 \\ \end{bmatrix}$

then, no matter what $P$ is, I'm going to wind up with $x$ innovations from the first sensor being correlated to $z$ innovations from the second, which seems intuitively wrong, if I'm interpreting this right.

Proceeding from here, my gain matrix ($K = P_{k|k-1} H^T S_k^{-1}$) winds up doing some pretty odd stuff (swapping rows and the like) so that, when updating a static system ($A = I_3, B = [0]$) with a constant measurement $\overrightarrow z = [1,0,0]$ I wind up with a predicted state $\hat x = [0,0,1]$.

If I separate the sensors and update the filter with each measurement separately, then $H H^T = I_3$, and I get sensible results.

I think I am confused about some technical points in one or more of these steps. Where am I going wrong? Does it not make sense to vertically concatenate the observation matrices?

I suppose that I could just set the off-diagonal 3x3 blocks of $S_k$ to 0, since I know that the sensors are independent, but is there anything in the theory that suggests or incorporates this step?

• Good question. I have never seen an implementation where the state variables are repeated as you do. Rather, much of the literature segments the sensor fusion logic as a process before inputting that data to the EKF. In 1982 NASA used this approach for sensor fault-tolerance decisions: ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/19820017360.pdf May 28 '16 at 16:47
• Doesn't that boil down to a weighted average of the 2 measurements with the weights being the inverse of the covariance? (as described here) Jan 16 '20 at 6:47
• Yes and no; if you substitute the identity in for most variables and ignore the prediction covariance, then the Kalman Filter does indeed boil down to a covariance-weighted average. But it is actually a generalization of this idea, which blends the observation covariance with the current estimate covariance into a value called the "innovation" in order to compute the optimal weighted average of measurements (aka the state estimate) alongside an updated state estimate covariance. In this case I was using a contrived example to build intuition for interpreting this innovation matrix. Aug 15 '20 at 19:33

Are you sure about your expression for $HH^T$?
$$HH^T = \begin{bmatrix} 1 & 0 & 0 & 1 & 0 & 0 \\ 0 & 1 & 0 & 0 & 1 & 0 \\ 0 & 0 & 1 & 0 & 0 & 1 \\ 1 & 0 & 0 & 1 & 0 & 0 \\ 0 & 1 & 0 & 0 & 1 & 0 \\ 0 & 0 & 1 & 0 & 0 & 1 \end{bmatrix}$$ which agrees with your intuition.