8
$\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?

$\endgroup$
  • 1
    $\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 May 28 '16 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 Jan 16 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$ – evenex_code Aug 15 at 19:33
2
$\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$
  • $\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$ – evenex_code May 31 '16 at 15:26

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

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