# Kalman Filter - Non Linear Measurement Model

I'm new to Kalman filters and estimation in general. I'm running a simple test of an EKF to check my understanding, but I'm getting some odd results with a particular case.

Given a state vector: $$\textbf{x} = \left(\begin{matrix} p_x \\ p_y \\ p_z \\ \end{matrix}\right)$$

If I have a sensor that measures these values directly, i.e.: $$\textbf{z} = \left(\begin{matrix} p_x \\ p_y \\ p_z \\ \end{matrix}\right)$$

Then I get a converging solution. However, if I decide that my sensor actually measures the square of each value, i.e. $$\textbf{z} = \left(\begin{matrix} p_x^2 \\ p_y^2 \\ p_z^2 \\ \end{matrix}\right)$$

Then following the process on the EKF wiki page causes my solution to diverge very rapidly. This seems to be irrespective of the dynamic model of the system, but entirely dependent on whether what the sensor is measuring is linear. Is there a standard approach to follow in this situation?

EDIT: Turns out it was a silly mistake on my part. I just implemented the filter incorrectly. The EKF was valid in this case.

• This seems like a mild nonlinearity in the output equation. I'll try to work an example tomorrow. The EKF should work reasonably well. – Peter K. Apr 14 '16 at 3:18
• @PeterK. Turns out it was a silly mistake on my part. I just implemented the filter incorrectly. If you're already working on an example, I'll leave the question open. – Andrei Khramtsov Apr 14 '16 at 12:09
• I have been! Let me see how far I get. Got some real-work stuff that needs doing. If you have time, perhaps you could self-answer? That's a perfectly legitimate way to resolve this. – Peter K. Apr 14 '16 at 12:13
• Apologies it's taken so long; this seems to do mostly the right thing. – Peter K. Apr 19 '16 at 15:12

So let's assume your model is: $$\textbf{x}_{k} = \textbf{x}_{k-1} + \textbf{n}_k$$ where $$\textbf{n}_k = \left(\begin{matrix} n_x \\ n_y \\ n_z \\ \end{matrix}\right)$$ and each of the $n_x,n_y,n_z$ are independent, zero-mean Gaussian noise terms with variances $\sigma^2_{nx},\sigma^2_{ny},\sigma^2_{nz}$.

The output equation is then: $$\textbf{z}_k = \textbf{f}(\textbf{x}_k) + \textbf{m}_k = \left(\begin{matrix} p_x^2 \\ p_y^2 \\ p_z^2 \\ \end{matrix}\right) + \textbf{m}_k$$ where $$\textbf{m}_k = \left(\begin{matrix} m_x \\ m_y \\ m_z \\ \end{matrix}\right)$$ and each of the $m_x,m_y,m_z$ are independent, zero-mean Gaussian noise terms with variances $\sigma^2_{mx},\sigma^2_{my},\sigma^2_{mz}$.

So following the EKF Wikipedia page, that means: $$\textbf{F}_{k-1} = \textbf{I}\\ \textbf{H}_{k} = {\tt diag}(2\textbf{x}_k)$$ where ${\tt diag}$ creates a square matrix from the vector argument.

Implementing this in R (code below) shows the follow example of the true state and the state estimate. As you can see, it has some problems because the output equation masks the sign of the state (that's why the top plot shows a mirror image around the zero axis).

R Code Below

#30103

# First, construct the signal model and generate the data.
T <- 1000

sigma_n <- 0.1
n_k <- array(rnorm(T*3,0, sigma_n), c(3,T))

sigma_m <- 0.1
m_k <- array(rnorm(T*3,0, sigma_m), c(3,T))

F <- array(c(1,0,0, 0, 1, 0, 0, 0,1),c(3,3))

x_0 <- array(1,c(3,1))
x <- array(0,c(3,T))
x[,1] <- x_0

z <- array(0,c(3,T))

for (k in 2:T)
{
x[,k] <- F %*% x[,k-1] + n_k[,k]
z[,k] <- ^(x[,k],2) + m_k[,k]
}

# Next, form the EKF
H <- 2* array(c(1,0,0, 0, 1, 0, 0, 0,1),c(3,3))

Q <- sigma_m^2*array(c(1,0,0, 0, 1, 0, 0, 0,1),c(3,3))
R <- sigma_n^2*array(c(1,0,0, 0, 1, 0, 0, 0,1),c(3,3))

library("MASS") # For pseudo inverse ginv()

xkm1km1 <- matrix(rep(0,3*T+3),3,T+1)
xkm1km1[,1] <- x_0

xkkm1 <- matrix(rep(0,3*T),3,T)
K <- array(rep(0,3*3*T),c(3,3,T))
Pkm1km1 <- array(0,c(3,3,T+1))
Pkm1km1[,,1] <- array(c(1000,0,0 ,0,1000,0, 0,0,1000), c(3,3))

zhat <- matrix(rep(0,3*T),c(3,T))
err <-  matrix(rep(0,3*T),c(3,T))

for (k in 2:T)
{
xkkm1[,k] <- F %*% xkm1km1[,k-1]
Pkkm1 <- F %*% Pkm1km1[,,k-1] %*% t(F) + Q

H <- 2*diag(xkkm1[,k])

K[,,k] <- Pkkm1 %*% t(H) %*% ginv( H %*% Pkkm1 %*% t(H) + R)
err[,k] <- z[,k] - H %*% xkkm1[,k]
xkm1km1[,k] <- xkkm1[,k] + K[,,k] %*% err[,k]
Pkm1km1[,,k] <- (matrix(c(1,0,0,0,1,0,0,0,1),3,3) - K[,,k] %*% H) %*% Pkkm1
zhat[,k] <- as.numeric(H %*% xkkm1[,k])
}

p1 <- x
p2 <- xkm1km1
lims <- c(-5,5)
par(mfrow = c(3,1), pty="m")
plot(p1[1,], col="grey",ylim=lims)
lines(p2[1,], col="red")
title("True (grey) and Estimated (red) State p_x")
plot(p1[2,], col="grey",ylim=lims)
lines(p2[2,], col="red")
title("True (grey) and Estimated (red) State p_y")
plot(p1[3,], col="grey",ylim=lims)
lines(p2[3,], col="red")
title("True (grey) and Estimated (red) State p_z")