# Kalman Filter for estimating position with nonconstant velocity & acceleration

I am trying to estimate the position & head direction of a rodent going through a 2D environment (a circular surface of 1m radius). Above his head is an overhead camera which records 4 LEDs mounted on his head. The position is calculated as the centroid of the LEDs positions and the head direction as the angle of the LEDs (see images).

The issue is that wires attached to his head occasionally occlude 1-4 of the LEDs and thus prevent/complicate measurement. Furthermore, the head direction estimation is particularly subject to lots of noise due to nonhorizontal head movements which add a 3rd dimension to the LED movements which are assumed to be parrelel to the surface (thus introducing a sort of parallax).

I thought a kalman filter would be a great way too smooth this data. However, in Matlab it seems that to implement this I would need to assume either constant acceleration or velocity which is not the case since the rodent is freely moving.

Is a standard Kalman filter still possible? Maybe a nonlinear (extended) KF? Are there any other better solutions?

Thanks Roman

Rodent in a square environment. I included this image since here the overhead wires and LEDs are more easily visible

Rodent in the circular environment. Notice that 1 of the LEDs (green) is occluded by the wire:

• Can you give more detail about how the LEDs are used? Your problem should be manageable using a standard KF, but it will depend on how the LED measurements translate to velocity and acceleration estimates. Also, some sample data would be useful. – Peter K. Dec 12 '15 at 17:26
• Can you please add a bit more detail regarding your setup? How does this 4 led detector work exactly? Is it mounted on the rodent? Why does its head movement seems to be a problem? Finally, why not a camera above the "2d environment" (presumably a maze?) with a much simpler way of detecting the centroid of a cluster of moving pixels? – A_A Dec 12 '15 at 17:33
• @PeterK I updated to add more information. What sort of sample data would be useful? I uploaded an image of the rat to give a better indication. – DankMasterDan Dec 14 '15 at 22:07
• @A_A see updates. Threre is an overhead camera, but its potentially occluded due to wires/obstacles – DankMasterDan Dec 14 '15 at 22:08
• Nice rat! OK. That makes it clearer. I'll see what I can do, but I'm traveling at the moment and it make it harder to answer questions with detail. – Peter K. Dec 15 '15 at 8:47

So this is just the start of an answer. I'll have to keep updating it as I go.

The first attempt is to say that the quantities you are interested in are the location of the center of the four LEDs, and the roll, pitch, and yaw (rotation angles) of the LEDs.

That means your Kalman FIlter state will be: $$\mathbf{x}_k = \left[x_k\ y_k\ \alpha_k\ \beta_k\ \gamma_k \right]^T$$ where $\mathbf{x}$ is the state vector, $x_k$ is the $x$ location, $y_k$ is the $y$ location, $\alpha_k$ is the roll, $\beta_k$ is the pitch, and $\gamma_k$ is the yaw.

For now, I'm assuming that we are not interested in estimating velocity, $v_k$., or acceleration, $a_k$.

Let's assume that the state update equation is then: $$\mathbf{x}_{k+1} = f(\mathbf{x}_{k}) + v_k = \mathbf{x}_{k} + v_k$$ where $f$ is the state update function, here $f(\mathbf{x}_{k}) = \mathbf{x}_{k}$ so $f$ is just multiplication by the identity, $\mathbf{I}$, $v_k$ is a $5 \times 1$ Gaussian noise vector with covariance matrix $$R = \left[ \begin{array}{ccccc} \sigma^2_x & 0 & 0 & 0 & 0\\ 0 & \sigma^2_y & 0 & 0 & 0\\ 0 & 0 & \sigma^2_\alpha & 0 & 0\\ 0 & 0 & 0 & \sigma^2_\beta & 0 \\ 0 & 0 & 0 & 0 & \sigma^2_\gamma\\ \end{array} \right]$$ i.e. the changes in all variables are just Gaussian (bandlimited) white noise. Note that this may not give the best results, but let's start there.

Then, the noise measurements will be the $(x,y)$ locations of the four LEDs: $$\mathbf{z}_k = \left[x^1_k\ y^1_k\ x^2_k\ y^2_k\ x^3_k\ y^3_k\ x^4_k\ y^4_k \right]^T$$ where $x^i_k$ for $i=1,2,3,4$ is the $x$ location of the $i^\mbox{th}$ LED at time $k$ and $y^i_k$ is the $y$ location of the $i^\mbox{th}$ LED at time $k$.

Then the output equation would be: $$\mathbf{z}_{k} = h(\mathbf{x}_k, r) + w_k$$ where $h$ is a function of the state and the radius of the circle that the LEDs sit on $r$. I have to go offline for a bit, so will fill in $h$ based on these equations later. Here, $w_k$ is an $8 \times 1$ noise vector with diagonal covariance matrix $Q_k$.

The idea where would be to make $Q_k$ time varying so that, when an LED disappears, the entries in the $Q_k$ matrix associated with those measurements have extremely large variances (meaning we are very unsure where the LED is). That should make the update just (effectively) keep the previous value with a minor update based on the new state estimate from the locations of the LEDs we CAN see.

In any event, it looks like your filter is going to be an extended Kalman filter (EKF) rather than a straight Kalman filter if we pursue this route.

The other option would be to use $(x^i_k,y^i_k)$ as the state variables so there's a more direct relationship between them and the measurements. However, then I'm not sure how to apply the fact that the four LEDs are in a set relationship to each other.

This is taking way longer than I expected, so I thought I'd just let you know I am trying to work on it...this week (and last weekend) were a dead loss for non-critical stuff.

The code below mainly just attempts to model the original measurements. No real attempt is made to use the EKF to do the estimation. Just using the mean location of the LEDs is currently all that it does.

Picture of an example track is below.

I'll try and get the EKF version going, but we'll see how this weekend goes.

R Code Below #27668

# Plot the data generated
plot_track <- function (xx,zz)
{
plot(xx[,1],xx[,2], type="l")

for (t in 1:T)
{
points(zz[t,1], zz[t,2], col="red", pch=18)
points(zz[t,3], zz[t,4], col="green", pch=18)
points(zz[t,5], zz[t,6], col="yellow", pch=18)
points(zz[t,7], zz[t,8], col="blue", pch=18)
}

points(xx[1,1],xx[1,2],col="magenta",lwd=20, pch=5)
points(xx[T,1],xx[T,2],col="orange",lwd=20)
}

# From a single x-y track, create all the locations for all the LED location measurements.
# Note that this just takes account of YAW, not pitch and roll.
{
dim(measurements) <- c(2,4)

yaw <- c(cos(yaw), -sin(yaw), sin(yaw), cos(yaw))
dim(yaw) <- c(2,2)

measurements <- yaw %*% measurements

# Reshape for later use in rbind
dim(measurements) <- c(1,8)

return(measurements)
}

# GIven the true (x,y) tracks in x, create the locations of the four LEDs
{
led_tracks <- data.frame(x1 = numeric(0), y1 = numeric(0), x2 = numeric(0), y2 = numeric(0), x3 = numeric(0), y3 = numeric(0), x4 = numeric(0), y4 = numeric(0))
for (t in 1:T)
{
led_tracks[t,] <- create_measurements(tracks[t,1], tracks[t,2], radius, tracks[t,3]);
}

return(led_tracks)
}

# Generation of the original data, Don't do it if z exists.
#if (FALSE == exists("led_tracks_true"))
{

Nstate <- 3 # States are X,Y, RYaw Angle
x0 <- rep(0,Nstate) #The state vector starts at all zeros

sigmas <- c(0.1,0.1,0.001)
Q <- diag(sigmas)

T <- 1000
x_true <- rep(0,Nstate*T) # The true states
dim(x_true) <- c(T,Nstate)

#A = [ 1 dt dt^2/2 0
#      0  1     dt 0
#      0  0      1 0
#      0  0      0 1];
dt <- 0.1
A <- matrix(c(1,0,0,0,dt,1,0,0,dt^2/2,dt,1,0,0,0,0,1),4,4)
B <- matrix(c(0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,1),4,4)

true_acceleration <- rnorm(T,0,1)

for (t in 1:(T-1))
{
for (n in 1:Nstate)
{
x_true[t+1,n] <- x_true[t,n] + rnorm(1,0, sigmas[n])
}
}

plot_track(x_true,led_tracks_true)
}

# Generate the measurements by adding some noise to the true led tracks
n_measurements <- 8
measurement_sigmas <- rep(0.1,n_measurements)
R <- diag(measurement_sigmas)

led_tracks_measured <-  rep(0,n_measurements*T)
dim(led_tracks_measured) <- c(T,n_measurements)

for (t in 1:(T-1))
{
for (n in 1:n_measurements)
{
led_tracks_measured[t+1,n] <- led_tracks_true[t,n] + rnorm(1,0, measurement_sigmas[n])
}
}

# Pick some times where we have an occluded LED
n_occlusions <- 50
occlusion_times <- sample(1:T, n_occlusions)
occlusion_leds <-  sample(1:4,T, replace = TRUE)

led_tracks_measured[occlusion_times, occlusion_leds] <- NA

# Attempt #1 : Just average the locations of the four LED tracks.
# When an LED is occluded, just ignore that measurement and average
# the remaining three LED locations.
est_avg <- rep(0,2*T)
dim(est_avg) <- c(T,2)

for (t in 1:T)
{
est_avg[t,1] <- mean(led_tracks_measured[t,c(1,3,5,7)], na.rm = TRUE)
est_avg[t,2] <- mean(led_tracks_measured[t,c(2,4,6,8)], na.rm = TRUE)
}

plot(est_avg - x_true[,1:2])

# Attempt #2 : An EKF approach?

• Peter K., this looks incredibly promising. Thank you! My only question is that the state equation has no state transition model (usually denoted by matrix F). Thus, dont we a priori assume that the rat ISNT moving? Would that make the KF work worse? – DankMasterDan Dec 15 '15 at 19:01
• Yes, we could add velocity terms to the state, and make the appropriate selection for the transition matrix (it's just $\mathbf{I}$, the identity matrix, now). I'm traveling, but will try to flesh it out a bit more tomorrow. – Peter K. Dec 15 '15 at 22:14
• Just a minor note that may be helpful: Why not model the 4 LEDs as 1 sensor? Then you get an indication of its "fix quality" WITHOUT a time variant component. 1 LED Visible (LV) you can only update position as the difference between the last good value and the new, 2 LV: Update with difference both position and rotation, (3 or 4) LV and you have a very good "absolute" position AND rotation estimate. So, the state vector is simplified (x,y,r) and the NLedsVisible are fed into the f(x) and potentially R too. Just a suggestion. – A_A Dec 22 '15 at 14:10
• @A_A I started to model this, but have been run off my feet with end-of-year closing of issues at work. I'm not sure why you'd include $r$ in the state vector? Or do you mean $r$ is the rotation angle, not the radius? That would be yaw ($\gamma$) in my model. – Peter K. Dec 22 '15 at 14:24
• @PeterK. No worries, I haven't had time too, to play around with this myself and it is an interesting one. Seeing you made a start, I thought I might just leave it as a comment in case it helps. Yes, r is yaw indeed. I went for one rotation because the 3D version is a headache for progressively worst cases of losing 1 LED, 2, 3, etc. – A_A Dec 22 '15 at 14:36

However, in Matlab it seems that to implement this I would need to assume either constant acceleration or velocity which is not the case since the rodent is freely moving.

First of all you can choose any dynamic model not only constant acceleration or velocity. Secondly, In Kalman filter you don't need to have exact dynamic model. consider state dynamic equation

$\mathbf{x}_k = \mathbf{F}\mathbf{x}_{k-1} + \boldsymbol{\nu}$

With your prior knowledge about the dynamic model of the system you specify transition matrix $\mathbf{F}$ and your uncertainty about this model is captured by the dynamic noise vector $\boldsymbol{\nu}$ which is a zero-mean multivariate Gaussian with covariance matrix $\mathbf{R}$. The more uncertain you are about $\mathbf{F}$ the larger the $\mathbf{R}$ you have to choose. The constant velocity or acceleration models are just the most basic dynamic models reflecting the nature lows of kinematics and most of the times they are good enough if you don't choose very small or very large $\mathbf{R}$.