10
$\begingroup$

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?

Improve this question
$\endgroup$
3
  • 3
    $\begingroup$ 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 $\endgroup$
    – SteveO
    Commented May 28, 2016 at 16:47
  • $\begingroup$ 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) $\endgroup$
    – Rufus
    Commented Jan 16, 2020 at 6:47
  • $\begingroup$ 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. $\endgroup$
    – fumigail
    Commented Aug 15, 2020 at 19:33

1 Answer 1

Reset to default
4
$\begingroup$

Are you sure about your expression for $HH^T$?

I get

$$ 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.

Improve this answer
$\endgroup$
1
  • $\begingroup$ Huh... yeah, you're right. Must have typed the expression in wrong. Multiple times, evidently. Next time I see something this weird I'll work it out by hand. $\endgroup$
    – fumigail
    Commented May 31, 2016 at 15:26

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.