I'm trying to simulate a particle going from (-3,0) to (3,0) with a constant velocity and some noise (e.g. the particle is a quadcopter trying to fly at constant velocity, but may be pushed by gusts of air). The model I have come up with is: $s_{n+1}=A s_n+B u_n+G w_n$ which in my case is: $$ \left[\begin{array}{l} x_{n+1} \\ y_{n+1} \\ \dot{x}_{n+1} \\ \dot{y}_{n+1} \end{array}\right]=\left[\begin{array}{cccc} 1 & 0 & \Delta t & 0 \\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array}\right]\left[\begin{array}{l} x_n \\ y_n \\ \dot{x}_n \\ \dot{y}_n \end{array}\right]+\left[\begin{array}{cc} \frac{\Delta t^2}{2} & 0 \\ 0 & \frac{\Delta t^2}{2} \\ \Delta t & 0 \\ 0 & \Delta t \end{array}\right] w_n $$
Where I don't have a control input (because it is constant velocity), and $w_n \sim N(0, Q)$ is the two dimensional velocity noise (so essentially a random acceleration) with $Q=\left[\begin{array}{cc}\sigma_{x x}^2 & 0 \\ 0 & \sigma_{y y}^2\end{array}\right]$ And I'm adding this into the state covariance matrix: $P_{n+1}=AP_{n}A^{T} + GQG^{T}$.
I think this is correct so far(?), but I'm having a bit of confusion about generating the data for my simulation.
- In MATLAB I'm generating the position vector in the following way:
wi=[-3;0];wl=[3;0];v_gnd=(wl-wi)./ts;
for i=1:ts;w_gnd(:,i)=w_gnd(:,i)+v_gnd+[normrnd(0,Qx);normrnd(0,Qy)];end
That is I choose an initial and last point, find the constant velocity needed between the two points by dividing by a time I want (ts), and adding 0-mean and Qx/Qy variance normal random noise to the position vector. However, two of my professors have told me that this is wrong. They say that the ground truth should be noiseless (i.e. just the straight line path from -3,0 to 3,0 with constant velocity), and that the process noise variance is only our belief of what the noise should be, but that the particle doesn't actually follow a random path. I am quite confused by this: if we consider the example of the quadcopter, it WILL be moved around by wind - wouldn't it's erratic path BE the ground truth?
- A separate question, but related to my simulation: My sensors are measuring ranges to the particle. To get position, I am using a maximum likelihood estimator (with a sensor noise model that I have derived) - and using that as the measurement vector in the KF. One of my professors thinks this is alright, but the other thinks that I should be using an EKF because the relationship between the range and position is non-linear. However, I think that even if the range-position relationship is non-linear, the KF is actually operating on the position measurements, so there is linear relationship in the tracking itself. Or do I need an EKF in this case?