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.
create_measurements <- function(x,y,radius,yaw)
{
measurements <- c(x,y+radius,x,y-radius,x+radius,y,x-radius,y)
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
create_all_tracks <- function(tracks, radius)
{
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"))
{
radius <- 0.1 # Radius of each LED ring
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])
}
}
led_tracks_true <- create_all_tracks(x_true,radius)
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?
