PreyModel_Kalman.Rmd

---
title: "Learning Kalman Filters, for Tracking Prey"
output:
  html_document:
    df_print: paged
---
This is an [R Markdown](http://rmarkdown.rstudio.com) Notebook, used for learning and testing ideas around Kalman Filters. 
I am reading Fundamentals of Kalman Filtering : A Practical Approach 3rd edition by Paul Zarchan and Howard Musoff [3], which offers example code in Fortran. I am reimplementing some examples for learning purposes here and keeping some notes on the way.
I am also looking across to the more practical guide in [1].

Any estimation problem consists of forming an estimate, here $\hat{x}$ of a hidden state via observable measurements, denoted $x^*$.

### Simulating White Noise
First relevant example I noticed, found on pg 30,is on simulating a simple low-pass filter that is driven under white noise. 
Power spectral density is given by the Fourrier transform of the autocorrelation function. 
One simple and useful form for the power spectral density is that of white noise, in which the power spectral density is constant or $\Phi_xx = \Phi_0$.
The autocorrelation function for white noise is a delta function given by $\phi_xx = \Phi_0 \delta(t)$ (white noise)
Although white noise is not physically realizable, it can serve as an invaluable approximation for situations in which a disturbing noise is wide bandwidth
compared to the system bandwidth.

The example here shows you how to scale the Gaussian noise source so as to approximate the relevant whitenoise *constant powersprectral density* (ex. $\Phi_0=1$), and second that it shows that it applies the 2nd Order Runge-Kutta to integrate the filter's evolution.

Make Low pass filter with pseudowhite noise:
\[y'=(x-y)/T\],
with transfer function in the s-domail : \[\frac{X(s)}{Y(s)} = K(s) =\frac{1}{1+sT} =  =\frac{1}{1+s/\omega_0} \], the filter's cut-off frequency being $\omega_0 = 1/T$.
If x is white noise, then the mean square value of the output of the filter can
also be shown to be : $E[y^2(τ)] = \Phi_0 (1-\exp(-2t/T))/2T$

The standard deviation of the pseudowhite noise (actual white noise over an infinite bandwidth has infinite standard deviation) is related to the desired white noise spectral density $\Phi_0$ and integration interval h according to $\sigma=\sqrt{(\Phi_0/h)}$, where h it the time step of the simulation and Phi_0 has dimensions of units squared per hertz.

The following is an example of a low passfilter output driven by by simulated whitenoise, used to confirm that 68% of responses should be within in sigma of the mean (here zero).

```{r}

tau = 0.2
Phi_0=1
t=0
h = 0.01 ##INtegration TimeStep
sig=sqrt(Phi_0/h) ##scaled Sigma for PseudoRandom White nOise
tLim = 4.999 ##Total Simulation Time
x = vector()
y = vector()
y[1] = 0.0
sigP= vector()
sigM= vector()

i=1
while (t < tLim)
{
  x[i]=rnorm(1,mean=0,sd=sig)
  yd=(x[i]-y[i])/tau
  
  y[i+1] = y[i] + h*yd
  t=t+h ##Increment Time
  ##Doing it 2ce as we are integrating using 2nd Order Runge-Kutta
  yd=(x[i]-y[i+1])/tau
  y[i+1]=(y[i]+y[i+1])/2.0+0.5*h*yd
  sigP[i] = sqrt( Phi_0*(1-exp(-2.0*t/tau) ) / (2*tau) )
  sigM[i] = -sigP[i]
  i=i+1 ##Increment Discrete time Sample index
}

plot(x,type='l',col="black",lty=3) ##Pseudo White Noise Input
lines(y,type='l',col="red",lw=2)
lines(sigP,type='l',lty=3,lw=3)
lines(sigM,type='l',lty=3,lw=3)
legend("topright",lty=c(3,2),col=c("black","red"),legend=c("WT in","Filter out"))
```

The low-pass filter is integrated with timestep h, while the integration is done using 2nd order runge-kutta, instead of simply using euler step derivatives. The 1sigma lines are enveloping the signal. Reducing tau increases the cut-off frequency of the filter, thus letting more noise through.
The integration interval is always chosen to be at least several times
smaller than the smallest time constant $\tau$ $(h << tau)$ in order to get correct answers with numerical integration techniques), the noise will look white to the system. Thus if the difference in  timescale whitenoise to the timestep is large, then it the expected output of the filter should fall within the 1sigma envelope 68%time.

## State Space Notation 

The standard way of representing these systems uses matrix notation :
\[\dot{\bf{x}}  = \bf{Fx} + Gu + w  \], where $u$ is the input vector and $w$ the noise.
The 1st order LP filter from above put in to this form would simply be F=-1/T,G=0,w=\eta/T.

When $F$ is time-invariant then there exists  a *transition * or fundamental matrix able to propagate the state forward from $t_k$ to $t_{k+1}$ as $x(t_{k+1})=\Phi(t_{k+1}-t_{k})x(t_k)$.
One simple way is through the inverse Laplace tranform of : 
\[ \Phi(t) = \mathcal{L}^-1[(sI-F)^-1]   \]
or via Taylor series expansion:
\[ \Phi(t) = \exp^Ft = I +Ft + (Ft)^2/2! + \cdots (Ft)^n/n! + \cdots  \]

Once we have the fundamental matrix we do not have to integrate differential equations to propagate the states forward.
We can propagate states forward by matrix multiplication.
In Kalman filtering we have to propagate state estimates ahead by one sampling time. 
If we have an exact expression for the fundamental matrix, we will be able to perform the propagation
by matrix multiplication. When an exact fundamental matrix is not available, we
will have to resort to numerical integration to propagate ahead the state estimates.

## Least Squares Fit - (the gaussian method) 
All of the filtering techniques that will be discussed in this text are,
one way or the other, based on Gauss’s original method of least squares.

1. we will assume a polynomial model to represent the actual signal.
2. we will try to estimate the
coefficients of the selected polynomial by choosing a goodness of fit criterion.
3. we will usecalculus to minimize the sum of the squares of the individual discrepancies in
order to obtain the best coefficients for the selected polynomial.

Let $\hat{x}_k$ be the estimated value and ${x}^*_k$ denote measured values. The sum squared distance measure is to be minimized:
\[R = \sum^n_{k=1}(\hat{x}_k-{x}^*_k)^2. \]
We then use standard calculus techniques and solve for 1st derivative of this series $\dfrac{dR}{d\hat{x}_k} = 0$ to be equal to zero, and the $\dfrac{d^2R}{d\hat{x}_k^2} > 0$.

This method operates as batch processing technique to fit a model to existing data, yet in real time operations estimates are often required as soon as the measurements are being taken.
For polynomial models of degree $n$, with coeffients $a_n$, we would need to solve the system of equations for each derivative $\dfrac{dR}{a_n} = 0$
Thus a matrix inverse has to be evaluated as part of the required computation.
The dimension of the matrix inverse was proportional to the order of the polynomial used to best fit the measurements in the least-squares 
sense.

For a Second-Order or Three-State Least-Squares Filter we have:
\[ \hat{x} = \alpha_0 + \alpha_1 t + \alpha_2 t^2 \]
\[ \hat{\dot{x} } = \alpha_1 + 2 \alpha_2 t\]
\[ \hat{\ddot{x} } = 2 \alpha_2 \]

which can be converted to the discrete-time version by letting $t=(k-1) T_s$.
and finally we minimize the residual sum between model and measurements:
\[R = \sum^n_{k=1}(\hat{x}_k-{x}^*_k)^2 = \sum^n_{k=1} [\alpha_0 + \alpha_1(k-1)T_s + \alpha_2(k-1)^2T^2_s - {x}^*_k]^2 \]

### Recursive Least-squares fitting algorithm 
The batch-processing method of least squares of can be made recursive.
Because the new least-squares filter is recursive, estimates are available as soon as measurements are taken. 

A zero-order (ie constant estimate) filter:
\[\hat{x}_{k+1} = \frac{ \sum_{i=1}^{k+1} x_i^*}{k+1} \]
after manipulating the series by letting \[  \sum_{i=1}^{k} x_i^* = k \hat{x}_k\]
it can then be expressed in the simple recursive form :
\[\hat{x}_{k+1} = \hat{x}_k + \frac{{x}^*_{k+1} - \hat{x}_k}{k+1} \]

If the measurement noise is a zero-mean Gaussian process with variance s 2 n , then a formula can also be derived that describes the
variance of the error in the filter’s estimate (i.e., variance of actual signal minus filter estimate) $x^*_k = x_k+u_k$,
with the variance of the estimate being \[P_k=\sigma_n^2/k \] (see derivation from recursive error), where $\sigma_n^2$ is the variance of the measurement noise.

Define truncation error $\epsilon$ as the difference between the true signal and the estimate:
\[\epsilon_k = x_k - \hat{x}_k \]

we can use this to estimate the theoretical $e_k$ when the signal model and fitted model differ (ex. estimation using zero-order when signal is generated by 1st order filter).
As more measurements are taken k gets larger, and the error in the estimate caused by the measurement noise decreases while the error in the estimate caused by truncation error increases. 
**In principle, for any particular numerical example there is an optimal value of $k$  that will minimize the errors in the estimates caused by both measurement noise and truncation error.**

### 1st order recursive filter

the two gains of a first-order or two-state recursive least-squares filter can be shown(see [1]) to be 
\[K_{1_k} = \frac{ 2(2k-1) }{ k(k+1) } \]
\[ K_{2_k} =  \frac{6}{k(k+1)T_s} \] for $k=1 \cdots n$.

If we think of the measurement as a position, then the estimates from this two-state filter will be position and
velocity (i.e., derivative of position). In other words, position is the first state, whereas the derivative of position or velocity is the second state.
The residual is :
\[ Res_k = x^*_k - \hat{x}_{k-1} - \hat{\dot{x}}_{k-1}T_s \]
The new filter estimates are a combination of the preceding state estimates
projected forward to the current time plus a gain multiplied by the residual or 
\[\hat{x}_{k} = \hat{x}_{k-1} + \hat{\dot{x}}_{k-1}T_s + K_{1_k} Res_k \] 
\[ \hat{\dot{x}}_{k} = \hat{\dot{x}}_{k-1} + K_{2_k} Res_k \]

Formulas can also be derived, using techniques similar to those used on the zeroth-order filter of the preceding section, describing
the variance of the error  in the estimates caused by noise in both states (i.e., $P_{11}$ and $P_{22}$ ) of the first-order filter.

Overall in all up to the three filters the recursive form has the same structure in that the new state estimate is always
the old state estimate projected forward to the current time plus a gain times a residual.

We can see from Figs. 3.34 and 3.35 that, from a measurement noise reduction point of view, it is best to use as low an order filter
as possible because that will tend to reduce the error in the estimate caused by measurement noise.
However, we also know from Table 3.4 that if we make the order of the filter too low there might be excessive truncation error (KL : model bias ).
*In each case the best-order filter to use will depend on the number of measurements to be
taken and the actual order of the signal.*
 

## General filter class : The g-h / $\alpha-\beta$ filters ( [2])

The *key insight* is that we form estimates from the measurement and prediction by forming some kind of blend of the prediction and measurement.
The Kalman and the least-squares filters belong to a general class of filters known as the g-h filter or the α-β filter. 
$g$ and $h$ refer to the two scaling factors that we used in our example.
$g$ is the scaling we used for the measurement  ($x^*$ in our example), and $h$ is the scaling for the change in measurement over time.
$\alpha$ and $\beta$ are  different names used for these same factors.

Each filter has a different way of assigning values to $g$ and $h$, but otherwise the algorithms are identical. For
example, the Benedict-Bordner filter assigns a constant to $g$ and $h$, constrained to a certain range of values.
Other filters such as the Kalman will vary g and h dynamically at each time step.
The general rules are: 

* Multiple data points are more accurate than one data point, so throw nothing away no matter how
inaccurate it is.
* Always choose a number part way between two data points to create a more accurate estimate.
* Predict the next measurement and rate of change based on the current estimate and how much we
think it will change.
* The new estimate is then chosen as part way between the prediction and next measurement scaled by
how accurate each is.

General Algorithm :

1. Initialization
    1. Initialize the state of the filter
    2. Initialize our belief in the state
2. Predict
    1. Use system behavior to predict state at the next time step
    2. Adjust belief to account for the uncertainty in prediction
3. Update
    1. Get a measurement and associated belief about its accuracy
    2. Compute residual between estimated state and measurement
    3. New estimate is somewhere on the residual line

The state is initialized with $\mathbb{x_0}$ , the initial estimate.
We then enter a loop, predicting the state for time or step $k$ from the values from time (or step) $k − 1$. We then get the measurement
$\mathbb{z_k}$ and choose some intermediate point between the measurements and prediction, creating the estimate $\mathbb{x_k}$.

Here is a generic implementation of such a filter:
```{r}

g_h_filter <- function(pdata,x0,dx,g,h,dt=1)
{
  x_est = x0
  res = vector()
  
  for(z in pdata)
  {
    ##Prediction Step
    x_pred <- x_est +(dx*dt)
    dx = dx
    
    #Update step
    residual = z -x_pred
    dx = dx +h*(residual/dt)
    x_est = x_pred + g*residual
    
    ##print(x_est)
    res = append(res,x_est)
  }
  ##print(res)
  return(res)
}
```

[1]:The g-h filter is not one filter - it is a classification for a family of filters.
The Benedict-Bordner filter was invented to minimize the transient error in this example, where $\dot{x}$ makes a step jump.
Eli Brookner in Tracking and Kalman Filtering Made Easy lists 11, and I am sure there are more.
Not only that, but each type of filter has numerous subtypes.
Each filter is differentiated by how g and h are chosen. So there is no ’one size fits all’ advice that I
can give here.
Some filters set g and h as constants, others vary them dynamically. 
*The Kalman filter varies them dynamically at each step.*
Some filters allow g and h to take any value within a range, others constrain one to be dependent on the other by some function $f (.)$ , where $g = f ( h )$ .

## Testing a g-h filter on prey motion data

We tracked N Rotifers from a single prey hunting experiment, and obtained the x-y positions of their blobs, between subsequent frames taken at 410 fps on average. Here we load this data set, add noise and examine the ability of a g-h filter to remove some abrupt random, and uncorrelated position noise.

First we load the data, and look at the distribution of speeds in terms of mm per sec
```{r}
##Test the filter with prey data
##Load Prey Data
## Load Prey Trajectory Data
source("config_lib.R") 
##Plot display Config 
line = 8.3; lineAxis = 3.0; cex = 1.4; adj  = -3; padj <- -7.2; las <- 1;
##Note It Returns Mean of Vector values centrered at x , with a window width k
meanf <- function(t,k) {n=length(t);tproc=t;k=min(k,n); for(i in (k/2):n) tproc[i]=mean(t[max(1,i-k/2): min(n, i+k/2) ]);  return(tproc)} #tproc=rep(NA,n);
## Returns the diff between a points in the vector and a point k positions ahead of it 
## Note result vector, shorter than t by  n-k
lagDiff <- function(t,k) {n=length(t);  k=min(k,n); tproc=vector(length = n-k);   for(i in 1:(n-k)){  tproc[i]= t[min(n, i+k)] -t[i]   };  return(tproc);};
mufps = G_APPROXFPS

#Location of prey motion track files, each containing tracklets from approx 20-30 rotifers 
setwd("/home/kostasl/workspace/zebrafishtrack/Rplots/")
loadPreyTracks <- function()
{
  preyFile = list.files("dat/",pattern=".csv"); ##list("AutoSet450fps_15-02-18_WTDryFed4Roti_4071_003_food_1.csv","AutoSet450fps_15-02-18_WTDryFed3Roti_4061_003_food_1.cs  v","AutoSet420fps_30-11-17_WTNotFed2Roti_252_006_food_1.csv","AutoSet450fps_14-06-18_WTNotFed5Roti_4491_001_food_1.csv")
  ldatPrey = list()
  n=1
  for (fileD in preyFile)
  {
    ldatPrey[[n]] = read.csv2(paste0("dat/",fileD),header=TRUE,sep = "\t",as.is =c(1,2,3,4,5,6,7),colClasses="numeric",dec="." )
    ##Modify PpreyID so there is no clash betweet tracked Items Between different Experiments
    if (n>1)
      ldatPrey[[n]]$FoodID = ldatPrey[[n]]$FoodID + max(ldatPrey[[n-1]]$FoodID);
    
  n = n + 1
  }
  datPrey <- do.call(rbind,ldatPrey)
  return(datPrey)
}
## Make Data File
##datPrey <- loadPreyTracks()
##save(datPrey,file="dat/preyTracks.RData")
##Load Data File
load(file="dat/preyTracks.RData")
## Show Prey Speed Histograms ##
vPreyIDs = as.numeric(names(table(datPrey$FoodID))) 
lPreySpeedDens = list()
lPreySpeedStat = list()

n = 1
for (pID in vPreyIDs){
  datPreyItem <- datPrey[datPrey$FoodID == pID,]  
  x0=as.numeric(head(datPreyItem$Centroid_X,1))+1
  y0=as.numeric(head(datPreyItem$Centroid_Y,1))+1
  
  vPreyDispl_mm = cumsum(sqrt( (diff(datPreyItem$Centroid_X)  )^2 + (diff(datPreyItem$Centroid_Y) )^2   )) *DIM_MMPERPX
  ## Speed estimate in terms of displacement between 2 points separated by 1 sec in time, starting from 1st point, againt pt 1 sec ahead of time  in terms   of Get Speed As displacement in A Second
  vPreySpeed_mm = lagDiff(vPreyDispl_mm,G_APPROXFPS)
  ##Get speed density when Prey Is moving (ie speed Above min)
  vMotSpeed <- vPreySpeed_mm[vPreySpeed_mm > 0.05]
  ## Select Only frames where prey is moving  
  if (NROW(vMotSpeed) > 10)
  {
    lPreySpeedDens[[n]] =density(vMotSpeed)
    lPreySpeedStat[[n]] <- list( mean=mean(vMotSpeed), sdE=sd(vMotSpeed)/sqrt(NROW(vMotSpeed))  )
    n=n+1
  }
}
datPreyStat <- data.frame(do.call("rbind",lPreySpeedStat))
  
## Plot Densities of Rotifer Speed
#plot(lPreySpeedDens[[1]],type='l')
#for (dens in lPreySpeedDens)
#  lines(dens);

## Plot Hist of Mean Prey Speeds
hist(unlist(datPreyStat[,1]),breaks=10,main=paste("mean prey speeds=",prettyNum( mean(unlist(datPreyStat[,1])) )," SEM",prettyNum( sd(unlist(datPreyStat[,1]))/sqrt(NROW(datPreyStat[,1]))  ) )  )


setEnvFileLocations("HOME") #HOME,OFFICE,#LAPTOP
#pdf(file= paste(strPlotExportPath,"/stat/stat_MeanPreySpeeds252_006.pdf" ,sep=""),width = 7,height = 7)
  plot(density(unlist(datPreyStat[,1]),bw=0.08),lwd=3,main=NA,xlab=NA,ylab=NA,cex=cex,cex.axis=cex,cex.lab=cex )
  mtext(side = 1, cex=cex, line = lineAxis,expression(paste("Mean prey travel speed (mm/sec)" ) )  ) 
  mtext(side = 2, cex=cex, line = lineAxis-0.5, expression(paste("Density" ) )  ) #(",lambda,")"
#dev.off()


```

Then we select a prey item at random, and add noise and filter :
```{r}
##Pick A random Prey
datPreyItem <- datPrey[datPrey$FoodID == sample(vPreyIDs,1),]

x0=as.numeric(head(datPreyItem$Centroid_X,1))+1
y0=as.numeric(head(datPreyItem$Centroid_Y,1))+1
dx=0.01
g= 1/100 ##Measurement Scaling - 2orders Larger than h works best 
h = 1/1000## Prediction Scaling - Gain Smaller when highly noisy environment
dt = mufps/3 ##Filter TimeStep

## Add Noise To Prey Position Measurements
N <- NROW(datPreyItem$Centroid_X)
NoisePoints = N/100
NoiseX = cbind(round(runif(n=NoisePoints,min=1,max=N)), rnorm(n=NoisePoints,mean=5,sd=30))
NoiseY = cbind(round(runif(n=NoisePoints,min=1,max=N)), rnorm(n=NoisePoints,mean=5,sd=30))
datBrokenX = datPreyItem$Centroid_X
datBrokenX[NoiseX[,1]] = datBrokenX[NoiseX[,1]] + NoiseX[,2]

datBrokenY = datPreyItem$Centroid_Y
datBrokenY[NoiseY[,1]] = datBrokenY[NoiseY[,1]] + NoiseY[,2]

datFilteredX = g_h_filter(datBrokenX,x0,dx,g,h,dt)
datFilteredY = g_h_filter(datBrokenY,y0,dx,g,h,dt)

#Plot the X movement Signal+Noise and Filtered
plot(datBrokenX,lty=1,type='l',lwd=3)
lines(datPreyItem$Centroid_X,lty=5,type='l',lwd=4,col="green")
lines(datFilteredX,lty=2,col="red",lwd=2)

## plot The Y movement Signals + Filtered 
plot(datBrokenY,lty=1,type='l',lwd=3)
lines(datPreyItem$Centroid_Y,lty=5,type='l',lwd=4,col="green")
lines(datFilteredY,lty=2,col="red",lwd=2)


## Plot 2D prey Motion and filtered signal 
plot(datPreyItem$Centroid_X,datPreyItem$Centroid_Y,xlim=c(0,500),ylim=c(0,500))
lines(datBrokenX,datBrokenY,lty=2,col="grey",lwd=1)
lines(datFilteredX,datFilteredY,lty=2,col="red",lwd=2)


```


These simple fits to data of tracked blob prey trajectories that have been corrupted by noise show that indeed even a simple application of
a g-h filter on each of x and y position trajectories separately can work, given low values of gains $g=1/10^2$ and lower prediction gain $h= 1/10^3$.
With larger values of $h$, for example h=1/10 the estimated signal becomes very sensitive to noise, and tracks the noisy signal, larger values of up to $g=1/10$ seem to still provide good signal tracking results, yet as $g=8/10$ increases then it becomes obvious that the noisy measurements are closely followed.

```{r}
g= 8/10 ##Measurement Scaling - 2orders Larger than h works best 
h = 1/1000## Prediction Scaling - Gain Smaller when highly noisy environment
datFilteredX = g_h_filter(datBrokenX,x0,dx,g,h,dt)

#Plot the X movement Signal+Noise and Filtered
plot(datBrokenX,lty=1,type='l',lwd=3)
lines(datPreyItem$Centroid_X,lty=5,type='l',lwd=4,col="green")
lines(datFilteredX,lty=2,col="red",lwd=2)

```

Letting $g=1/10$ and increasing the $h = 8/10$, has similar effects, only now it is the prediction  that is dominated by the noise. 

```{r}
g= 1/10 ##Measurement Scaling - 2orders Larger than h works best 
h = 8/10## Prediction Scaling - Gain Smaller when highly noisy environment
datFilteredX = g_h_filter(datBrokenX,x0,dx,g,h,dt)

#Plot the X movement Signal+Noise and Filtered
plot(datBrokenX,lty=1,type='l',lwd=3)
lines(datPreyItem$Centroid_X,lty=5,type='l',lwd=4,col="green")
lines(datFilteredX,lty=2,col="red",lwd=2)
```


*I implemented the g-h filter algorithm to the tracking of prey in the behavioural tracker*. It works well to remove sudden changes to the prey position,  and in addition it can be used to predict prey motion, based on last $\dot{x}$ estimates.
Yet, if the signal/blob position is lost for while and itdeviates away, then when it is found again we see oscillations of the estimated position about the real prey position. This is is effectively the same as having an $x_0$ that is far from the true signal, and initially oscillates before it settles.



## Estimate how prey speed affect predator aiming against capture distance
Given the above prey speed estimates, we can calculate how much such speeds can effect a larva's ability to accurately aim towards a rotifer it is tracking down, for a distance capture distance from where it executes its capture manoeuver. Assume larva move straight towards its heading angle $\theta$ when executing a capture bout, while prey is located at angle $\phi$.

\[
\frac{\partial\theta}{\partial r} = -\frac{\beta}{r^2 \sqrt{1-\sin^2(\theta) }} \nonumber\\
 = -\frac{\beta}{r^2 \sqrt{1- \frac{\beta^2}{r^2} } } 
\]
and with horizontal motion, $beta$ as 
\[
 \frac{1}{ \sqrt{(1 - b^2 /r^2) }  r}
\]

Next we compare how the angle to prey, and thus the aiming, during an interbout interval of duration $t_b$, under prey motion with speed $s_p$ moving horizontally across the visual field of the larva.
Given the prey travels $\beta$ distance during the interbout interval, changing the distance $r$ to it can severly affect the change azimuth to the prey, when the prey is very close to the larva.
```{r}
source("config_lib.R")
## Function using the derivative of Prey Azimuth as distance to prey changes for a prey located preyspeed*PauseTime Off Midline axis 
DerivAimingAngleWithDistance_dthetadr <- function(preyspeed,boutInterval,captureDistance,PreyAzimuth)
{
  r <- captureDistance
  beta <- r*sin(PreyAzimuth)  #preyspeed*boutInterval
  return( -beta/( (r^2)*sqrt(1-  (beta^2)/(r^2) ) ))
}
## Using the derivative wrt to beta, to estimate change in theta
DerivAimingAngleWithPosition_dthetadbeta<- function(preyspeed,boutInterval,captureDistance,PreyAzimuth)
{
  r <- captureDistance 
  b <- r*sin(PreyAzimuth) #r*sin(PreyAzimuth) 
 # res <- -(b^2)/( ((1 - (b^2) /(r^2) ) ^(3/2)) * (r^4) ) - 1/ ( sqrt(1 - (b^2) /(r^2) ) * r^2)
  res <-   1/ ( sqrt(1 - (b^2) /(r^2) ) * r)
  return(res )
}

## This directly calculates the change in Prey Azimuth Between two positions defined prior and After BoutInterval, given prey Speed
DeltaAimingAngleAgainstPreyMotion <- function(preyspeed,boutInterval,captureDistance,PreyAzimuth)
{
  b_init = captureDistance*sin(PreyAzimuth) #r*sin(PreyAzimuth)  X
  alpha_init = captureDistance*cos(PreyAzimuth) ##Y - Assume This does not change with prey motion - Only B
  b_final = b_init+preyspeed*boutInterval ## Move Prey Horizontally By distance of Speed*Time
  r_final = sqrt(b_final^2 + alpha_init^2)
  PreyAzimuthFinal = asin(b_final/r_final)
  return( (PreyAzimuthFinal-PreyAzimuth) )
}

plotPreyDistanceSpeedResults <- function()
{
  
 plot(mp$r,DeltaTheta,xlab=NA,ylab=NA,xlim=c(0.0,0.6),cex.axis=cex,cex=0.7,ylim=c(0.0,80),type="p",lty=2,pch=20 ) #
  ##  plot Across different Speeds
  for (speed in s)
  {
    idx = which(mp$preyspeed == speed)
    lines(mp$r[idx],y[idx],xlab="Prey distance mm",
          xlim=c(0.0,0.6),pch=3,
          main="Compare Theory To Δθ",
          col=colSp[which(s == speed)]) ##Theory
    points(mp$r[idx],DeltaTheta[idx],pch=20,col=colSp[which(s == speed)])
  }
##Plot Heat Map Legend
    x <- tail(r,1)/2+(1:NROW(s)/90) ##Make Narrow 1/2 length bar
    points(x,rep(60,NROW(x) ),pch=15,col=colSp,cex=1.8)
    text( x[1]+(x[1]-tail(x,1))/2 +strwidth("Prey speed")*0.6  ,65,labels="Prey speed",col="black",font=2.2)  ##0 V Angle
    text(x[1],55,labels=s[1],col="black",font=2.2)  ##0 V Angle
    txtW <- strwidth(paste(s[1])) /2 ##Text Width For Centre Aligment 
    text(tail(x,1)+txtW/2,55,labels=tail(s,1),col="black",font=2.2)  ##0 V Angle
    ## Axis Labels ##
    mtext(side = 1, cex=cex, line = lineAxis,expression(paste("Initial prey distance r (mm)" ) )  ) 
    mtext(side = 2, cex=cex, line = lineAxis-0.5, expression(paste("Change in prey azimuth (", Delta, theta^o,")") )  ) 
    legend("topright",title=NA,
         legend=c( paste("Calculated"), #(Bw:",prettyNum(digits=2, pdistBW ),")" ) ,
                   paste("Linearized Model " ) ) ,
         col=c("black"),lwd=c(3,1,1),pch=c(20,NA),lty=c(NA,1,2),horiz=TRUE,bty="n"	) 
    
}
  
preyspeed = 1 ##Mm/sec
boutInterval = 0.200
captureDistanceNear = 0.2 ##mm
captureDistanceFar = 0.45 ##mm

PreyAzimuthNear =  asin((preyspeed*boutInterval)/captureDistanceNear)  ##20*(pi/180) ##Larva's Initial Aiming angle prior to Prey Moving 
PreyAzimuthFar =  asin((preyspeed*boutInterval)/captureDistanceFar)  ##20*(pi/180) ##Larva's Initial Aiming angle prior to Prey Moving 
PreyAzimuth = 0*(pi/180)

## Dtheta/Dr ##
print(paste0("dtheta/dr at small distance ",captureDistanceNear,"mm : ",DerivAimingAngleWithDistance_dthetadr(preyspeed,boutInterval,captureDistanceNear,PreyAzimuth)*(180/pi)) )
thetaNear <- asin(preyspeed*boutInterval/(captureDistanceNear) ) *180/pi
DthetaDr_Near <- (asin(preyspeed*boutInterval/(captureDistanceNear) ) - asin(preyspeed*boutInterval/(captureDistanceNear-captureDistanceNear*0.01) )) *180/pi
print(paste0("dtheta/dr at 2x distance ",captureDistanceFar,"mm : ", DerivAimingAngleWithDistance_dthetadr(preyspeed,boutInterval,captureDistanceFar,PreyAzimuth)*(180/pi) ) )

##Calc R after prey from Distance Near, Moves by preyspeed*boutInterval
R_near <- sqrt(captureDistanceNear^2+(preyspeed*boutInterval)^2)
R_far <- sqrt(captureDistanceFar^2+(preyspeed*boutInterval)^2)
DThetaDBeta_Near <-  DeltaAimingAngleAgainstPreyMotion(preyspeed,boutInterval,captureDistanceNear,PreyAzimuth)*(180/pi)
DThetaDBeta_Far <- DeltaAimingAngleAgainstPreyMotion(preyspeed,boutInterval,captureDistanceFar,PreyAzimuth)*(180/pi)
##Compare To Calculated From Equation
print(paste0("Change in theta at ",captureDistanceNear,"mm (Near)  Dist: ", DThetaDBeta_Near ) );
print(paste0("Change in theta at ",captureDistanceFar ,"mm (Far) Dist: ",  DThetaDBeta_Far)) ;
DthetaDr <- DThetaDBeta_Near - DThetaDBeta_Far
print(paste("Calculated Dtheta/Dr :",DthetaDr))


## Calc derivatives Across different R, And prey Speeds
preyspeedR = range(0.1,1) ##Mm/sec
#r <- seq((preyspeed*boutInterval),(preyspeed*boutInterval*5),by=1/20) ##Use Speed to Relate to R
r <- seq((0.05),1.0,by=1/20) #Distances to Prey Use Speed to Relate to R
s <- seq(preyspeedR[1],preyspeedR[2],by=(preyspeedR[2]-preyspeedR[1])/5) ##Prey Speed Range
mp <- expand.grid(r=r,preyspeed=s) ##Make All comb or r and s
DeltaTheta = vector() ##Holds the Sampled Dtheta
y = vector() ##Holds the Linearized model Results
predError = vector()
for(i in  1:NROW(mp) )
{
  pPreyAzimuth =  asin( ( mp[i,]$preyspeed*boutInterval) / (mp[i,]$r) )
  dthetadr <- DerivAimingAngleWithDistance_dthetadr((mp[i,]$preyspeed),boutInterval,(mp[i,]$r),PreyAzimuth)
  dthetadb <- DerivAimingAngleWithPosition_dthetadbeta((mp[i,]$preyspeed),boutInterval,(mp[i,]$r),PreyAzimuth)
  b_init = captureDistance*sin(PreyAzimuth) #r*sin(PreyAzimuth)  X
  Db <- (mp[i,]$preyspeed*boutInterval)
  DrDb <- 1/sqrt(Db^2+mp[i,]$r^2)###(sqrt(Db^2+mp[i,]$r^2)-mp[i,]$r)/Db ##Get Change In R, due to Dbeta - R Increases with Db
  ##Estimate Angle Change Using PartDeriv, assuming Change in Dr 
  y[i] <- (180/pi)*(dthetadr*DrDb + dthetadb)*Db ##*cos(PreyAzimuth
  DeltaTheta[i] <- DeltaAimingAngleAgainstPreyMotion((mp[i,]$preyspeed),boutInterval,(mp[i,]$r),PreyAzimuth)*(180/pi)
  predError[i] <- y[i] - DeltaTheta[i] ##Calculate Distance Between Prediction And Actual
}


paletta <- colorRampPalette(rev(brewer.pal(9,'YlOrRd')));
colSp <- c(paletta(9),"#FF0000");

  plot(mp$r[idx],y[idx],xlab="Prey distance mm",
          xlim=c(0.0,0.6),pch=3,
          main="Compare Theory To Δθ",
          col=colSp[which(s == speed)]) ##Theory
  ##  plot Across different Speeds
plot(mp$r,predError)
  for (speed in s)
  {
    idx = which(mp$preyspeed == speed)
    points(mp$r[idx],predError[idx],pch=20,col=colSp[which(s == speed)])
  }
##Calc Avg Pred Error for points r > min Distance
print(sum(predError[which(mp$r > 0.2)]/NROW(which(mp$r > 0.2))))
print( paste("maximum pred error:", max(predError[which(mp$r > 0.1)] ) )  )
hist(predError[which(mp$r > 0.1)])
### plot Linearized Model and Sampled DeltaTheta from Positions Geometry Across speeds and Distance
plotPreyDistanceSpeedResults()

##Export To File
setEnvFileLocations("HOME") #HOME,OFFICE,#LAPTOP
pdf(file= paste(strPlotExportPath,"/stat/PreyAzimuthVsPreySpeedAndDistance.pdf" ,sep=""),width = 7,height = 7)
 plotPreyDistanceSpeedResults()
dev.off()

```


## A Bayesian discrete filter
Baysian probability theory allows us to compute probabilities of being in state $x_i$, at time $i$, given a measurement $z$ by utilizing the inverse: the probability of measuring $z$ given we are in state $x_i$.
We write Bayes formulation of updating posterior probabilities in terms of distributions $p(x)$ applied to a tracking problem. we let  
$x_i$ denote the position at step $i$, and $z$ be the measurement. Hence, we want to know $\mathcal P ( x_i | z )$ , that is, the probability
of target (prey) being at $x_i$, at step $i$ given the measurement $z$.
\[
p(x_i | z) = \frac{p(z|x_i)p(x_i)}{p(z)}.
\]
which often also appears as :
\[
p(A| B) = \frac{p(B|A)p(A)}{\int p(B|A_j)p(A_j)dA_j},
\]
with the denominator being usually impossible to compute, but in practice it just a normalization term over which we can sum.
$p(z|x_i)$ represents the likelyhood ($\mathcal{L}$) pdf (pmf) of given a measurement $z$ at $x_i$. 
The denominator $p ( z )$, is the probability of getting the measurement $z$ without taking the location into account. It is often called the evidence. We compute that by taking the sum of $x$, which is  how we compute the normalization! 

Let $||f(x)||$ be the normalized of $f(x)$, so as it becomes a pdf.
Then the filter equations are :

\[\mathbf{\hat{x} = x * f_x(.)}    \text{ Predict Step} \]
\[\mathbf{x = || \mathcal{L} . \hat{x} ||} \text{ Update Step} \]
where, $\mathbf{\hat{x}}$ is the prior ,$f_x(.)$ is the process model.

The algorithm is :

1.Initialization
  1. Initialize our belief in the state
2.Predict
  1. Based on the system behavior, predict state for the next time step
  2. Adjust belief to account for the uncertainty in prediction
3. Update
  1. Get a measurement and associated belief about its accuracy
  2. Compute how likely it is the measurement matches each state
  3. Update state belief with this likelihood


## Polynomial Kalman Filters 
(combined from both [1] and [3])
 Kalman filter equations *assume the noise is normally distributed*, and perform sub-optimally if this is not true. Designers for mission critical filters, such as the filters on space-craft, need to master a lot of theory and empirical knowledge about the performance of the sensors on their
spacecraft.

In this section we learn how to use Gaussians to implement a Bayesian filter. Instead of the g-h filter view, Kalman filter can also be seen as a Bayesian filter that uses Gaussians.

Earlier, we saw that the recursive least-squares filter provided the exact same estimates, for a given number of measurements, as the batch-
processing least-squares filter.
Here we will first provide the general equations for the discrete Kalman filter and then show under what conditions it is
completely equivalent to the recursive least-squares filter. 
We will also demonstrate the divergence problem with the Kalman filter, but we will show that there are a
variety of engineering fixes. 

To apply Kalman-filtering theory, our model of the real world must be described by a set of differential equations. These equations must be cast in
matrix or state-space form as :
\[ \mathbf{ \dot{x}} = \mathbf{Fx + Gu + w} \], where x is a column vector with the states of the system, $F$ the system dynamics
matrix, $u$ is a known vector, which is sometimes called the control vector, and $w$
is a white-noise process, which is also expressed as a vector.
There is a process-noise matrix $\mathcal{Q}$ that is related to the process-noise vector according to : 
\[\mathbf{Q} = \mathbb{E}[\mathbf{ww^T}] \],
which is sometimes used as a device for telling the filter that we know the filter’s model of the real world is not precise.
The Kalman-filter formulation requires that the measurements be linearly related to the states according to :
\[  \mathbf{z} = \mathbf{Hx +v}  \]
where $ \mathbf{z} $ is the measurement vector, $ \mathbf{H}$ is the measurement matrix that relates the state to the measurement, and $ \mathbf{v}$ is white measurement noise, which is also expressed as a vector.


The discrete Kalman-filtering equation is given by : 
\[ \mathbf{ \hat{x}_k } = \Phi_k  \hat{x}_{k-1} + G_k u_{k-1} + K_k(z_k - H \Phi_k \hat{x}_{k-1} - H G_k u_{k-1})\]
where $\Phi_k$ is the discrete version of the fundamental matrix (see sections above) evaluated as $\Phi_k = \Phi(T_s) $,
where $K_k$ represents the Kalman gain matrix and $G_k$ is obtained from 
\[G_k = \int_0^{T_s} \Phi (\tau)G d\tau \]
if $u_{k-1}$ is assumed to be constant between sampling instants.
The Kalman gains are computed, while the filter is operating, from the matrix Riccati equations.
The Riccati equations are a set of recursive matrix equations given by :
\[ M_k = \Phi_k P_{k-1} \Phi^T_k + Q_k \]
\[K_k = M_k H^T (HM_k H^T + R_k)^{-1}\]
\[ P_k = I - K_k H)M_k \]
where $P_k$ is a covariance matrix representing errors in the state estimates (i.e., variance of truth minus estimate) after an update and M k is the covariance matrix representing errors in the state estimates before an update.
The discrete process-noise matrix $Q$ k can be found from the continuous process-noise matrix $Q$ and the
fundamental matrix according to
\[ Q_k = \int^{T_s}_0 \Phi(\tau)Q \Phi(\tau) d\tau\] ( KL:Corrected for  $d\tau$ )
To start the Riccati equations, we need an initial covariance matrix $P_0$.

The gain of the Kalman filter *is chosen to minimize the variance of the error in the estimate.*
The Riccati equations are simply an iterative way of finding the optimal gain at each time
step.

A polynomial Kalman filter with zero process-noise and infinite initial covariance matrix is identical to
a recursive least-squares filter of the same order.
Having an initial covariance matrix that is infinite means that we have no a priori information on how to initialize the filter state.
Thus, the first gain of first-order polynomial Kalman and recursive least-squares filters is identical.
 
## Testing a filter on prey motion data

Here I load prey data and re-implemend the model I made for the VR prey stimulus screen, and use it as my prediction/plant/process model.
```{r}

## Show Prey Trajectory
plot(datPreyItem$Centroid_X,type="l",ylim=c(0,480)) ##,xlim=c(0,580),
lines(datPreyItem$Centroid_Y,type="l",col="blue",lty=2)


```

## References 

1. Kalman and Bayesian Filters in Python, Roger R Labbe Jr
2. Morrison, N., Introduction to Sequential Smoothing and Prediction, McGraw–Hill,
New York, 1969, pp. 339–376.
3. Fundamentals of Kalman Filtering: A practical approach, 3rd ed., Paul Zarchan, Howard Musoff

## Appendix 

#### VR Prey Model 
Code for virtual prey model projected on a 2D matrix LED display driven by Arduino.

```{cpp}
// \author : Konstantinos Lagogiannis March 2020
// \bref : firmware for displaying virtual prey on an NeoPixel LED Matrix 
// to conduct experiments on larval zebrafish hunting.
// A moving dot represents prey. Prey is modelled using kinematic equations for heading and propulsion force (rotifer motor drive)

#include <Gaussian.h> //For Drawing Random Gaussian No. /Note: If it gives Math.h missing, edit the Gaussian.h header and replace with "math.h"
#include <Adafruit_GFX.h>
#include <Adafruit_NeoMatrix.h>
#include <Adafruit_NeoPixel.h>
#ifndef PSTR
 #define PSTR // Make Arduino Due happy
#endif

#define PIN 6
#define N_PREY 5
#define MAX_VELOCITY 0.05
#undef _DEBUG
const int screenW = 44;
const int screenH = 11;

static inline int8_t sgn(int val) {
 if (val < 0) return -1;
 if (val==0) return 0;
 return 1;
}

// MATRIX DECLARATION:
// Parameter 1 = width of NeoPixel matrix
// Parameter 2 = height of matrix
// Parameter 3 = pin number (most are valid)
// Parameter 4 = matrix layout flags, add together as needed:
//   NEO_MATRIX_TOP, NEO_MATRIX_BOTTOM, NEO_MATRIX_LEFT, NEO_MATRIX_RIGHT:
//     Position of the FIRST LED in the matrix; pick two, e.g.
//     NEO_MATRIX_TOP + NEO_MATRIX_LEFT for the top-left corner.
//   NEO_MATRIX_ROWS, NEO_MATRIX_COLUMNS: LEDs are arranged in horizontal
//     rows or in vertical columns, respectively; pick one or the other.
//   NEO_MATRIX_PROGRESSIVE, NEO_MATRIX_ZIGZAG: all rows/columns proceed
//     in the same order, or alternate lines reverse direction; pick one.
//   See example below for these values in action.
// Parameter 5 = pixel type flags, add together as needed:
//   NEO_KHZ800  800 KHz bitstream (most NeoPixel products w/WS2812 LEDs)
//   NEO_KHZ400  400 KHz (classic 'v1' (not v2) FLORA pixels, WS2811 drivers)
//   NEO_GRB     Pixels are wired for GRB bitstream (most NeoPixel products)
//   NEO_GRBW    Pixels are wired for GRBW bitstream (RGB+W NeoPixel products)
//   NEO_RGB     Pixels are wired for RGB bitstream (v1 FLORA pixels, not v2)


// Example for NeoPixel Shield.  In this application we'd like to use it
// as a 5x8 tall matrix, with the USB port positioned at the top of the
// Arduino.  When held that way, the first pixel is at the top right, and
// lines are arranged in columns, progressive order.  The shield uses
// 800 KHz (v2) pixels that expect GRB color data.
Adafruit_NeoMatrix matrix = Adafruit_NeoMatrix(screenW, screenH, PIN,
  NEO_MATRIX_TOP     + NEO_MATRIX_LEFT +
  NEO_MATRIX_ROWS + NEO_MATRIX_ZIGZAG,
  NEO_GRB            + NEO_KHZ800);

// Define Basic Colour palette
const uint16_t colors[] = {
  matrix.Color(50, 50, 50),matrix.Color(200, 200, 200),matrix.Color(255, 0, 0), matrix.Color(0, 255, 0), matrix.Color(0, 0, 255) };

const uint16_t BGCOLOUR = matrix.Color(20, 10, 10);

typedef struct Point {
  Point(){X=0.0f;Y=0.0f;}
  Point(float iX, float iY){X=iX;Y=iY;}
  float X;
  float Y;
} t_point;


typedef struct Prey {
  public:
  t_point current_position;
  t_point previous_position;
  t_point velocity;
  float omegaDeg;
  float headingTheta;
  float df_propulsion;
  float df_friction;
  float mass;
  float dTheta;
  uint16_t color;
  uint16_t BGcolor;
  
  //Default Constructor
  Prey()
  {
     current_position.X = 22;
     current_position.Y = 5;
     headingTheta = M_PI;
     velocity.X = 0.0;
     velocity.Y = 0.0;
     df_propulsion = g2prop.random(); //(float)random(-50,50)/1000.0f;
     df_friction = 0.75;  /// Water Drag Force Coefficient / (prop to velocity)
     omegaDeg = 0;
     mass    = 1.0; //Assume unity mass initially
     color   = matrix.Color(random(60,250), random(20,250), random(20,250));
     BGcolor = BGCOLOUR; //matrix.Color(random(200,250), random(200,250), random(10,10));
  }


  
  void move()
  {
    //Draw Random Heading Change (Degrees)
    dTheta = gTurn.random();//(float)random(-2,-2)/100.0;
    //Update Angular Speed //With turn Drag
    gTurn.setMean(5.0); //Reset Mean

    //bounce off the border
    if (current_position.X >= (matrix.width()-1)|| current_position.X <= 0)
    {
      velocity.X = -(df_propulsion/mass)*cos(headingTheta);
      dTheta += 90;
      gTurn.move(40);//Change the mean turning angle randomly
      //gTurn.move(sgn(velocity.X)*0.001 ); //Change the mean turning angle randomly
    }
    if (current_position.Y >= (matrix.height()-1) || current_position.Y <= 0 )
    {
      
      velocity.Y = (df_propulsion/mass)*sin(headingTheta);
      gTurn.move(40);//Change the mean turning angle randomly
    }
    omegaDeg +=dTheta-df_friction*omegaDeg*0.5; //Update Angular Speed //Assume A Turn Drag too
    omegaDeg = max(-25,min(omegaDeg,25)); //Maximum Angular Velocity
    
    //Update Heading in Rads
    headingTheta += omegaDeg*(M_PI/180.0);//% (float)TWO_PI;
    headingTheta = (float)((int)(headingTheta*100.0) % (int)(TWO_PI*100.0))/100.0;
    
    // Draw random in propulsion (Random 2D walk)
    
    
    //Calc velocity update with Drag Forces
    velocity.X += max(-MAX_VELOCITY,min( (df_propulsion/mass)*cos(headingTheta)-df_friction*pow(velocity.X,1),MAX_VELOCITY) ) ; 
    velocity.Y += max(-MAX_VELOCITY,min( (df_propulsion/mass)*sin(headingTheta) -df_friction*pow(velocity.Y,1),MAX_VELOCITY) ) ; 
    
    current_position.X += velocity.X ;
    current_position.Y += velocity.Y;



    //Bound Position
    current_position.X = max(0.0,min(current_position.X,matrix.width()-1));
    current_position.Y = max(0.0,min(current_position.Y,matrix.height()-1));

    
  }

  // Renders the prey to The LED matrix screen
  void draw()
  {
  
    // Reset Previous Position TO BG
    matrix.drawPixel(previous_position.X,previous_position.Y,BGcolor);
    // Draw New Prey Position
    matrix.drawPixel(current_position.X,current_position.Y,color);
  
    previous_position = current_position;
  }
  
  private:
  //Gaussian Distribution for random Turn Angles (Degrees)
  Gaussian gTurn = Gaussian(random(-1,1), 15); //Let there Exist Biased Prey for turn
  Gaussian g2prop = Gaussian(1, 0.01);
  
  
} t_prey;




t_prey prey[N_PREY];



void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  Serial.print("\n\nStarting Virtual Prey driver programs for N:");
  Serial.println(N_PREY);
  
  matrix.begin();
  matrix.setTextWrap(false);
  matrix.setBrightness(5);
  matrix.setTextColor(colors[0]);
  randomSeed(analogRead(0));
  matrix.fillScreen(0); //colors[0]

  //Initialize Prey
  for (int i=0;i<N_PREY;i++)
  {
    prey[i].current_position.X = 22;
    prey[i].current_position.Y = 5;
    //Randomize Initial Heading
    prey[i].headingTheta = random(-M_PI*10.0,M_PI*10.0)/10.0; //Initial Heading clockwise,0Rad is right, pi/2 -> down, PI -> left
    
     Serial.print(prey[i].df_propulsion );//= 0.3; //Set Initial Propeller Force
    
  }

}

void loop() {
  // put your main code here, to run repeatedly:
 //matrix.fillScreen(colors[0]);
 

 // Top Left Mark Pixel
 matrix.drawPixel(0,0,colors[3]);
 
  //for (int i=0;i<matrix.width();i++)
 {
  //matrix.setBrightness(70);
  
  //newPt.X = max(0,min(oldPt.X + dX,matrix.width()));
  //newPt.Y = max(0,min(oldPt.Y + dY,matrix.height()));
  for (int i=0;i<N_PREY;i++)
  {
    prey[i].move();
    prey[i].draw();

  }
    
 #ifdef _DEBUG
  Serial.print(prey[0].headingTheta*180.0/M_PI);
  Serial.print("\t");
  Serial.print(prey[0].omegaDeg);
  Serial.print("\t");
  Serial.print(prey[0].dTheta);
  Serial.print("\t");
  Serial.println(sqrt(pow(prey[0].velocity.X,2)+pow(prey[0].velocity.Y,2)) );
 #endif
  
  matrix.show();
  delay(100);
 }
 
  //setCursor(x, 0);


}
```