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

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