How to generate ground truth in constant velocity Kalman simulation?

Question

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.

1. 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?

2. 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?

Answer 1

OK. Your first problem is both technical and political.

Technical

Technically, I agree with you -- the states you want to model and estimate should be the actual states of your physical system, and those actual states do follow a random path. If they followed a perfectly deterministic path, then your process variance matrix would be $\mathbf Q = \mathbf 0$.

---

Political

However, politically, you have two authority figures who disagree with you. Even in a perfect world you need to approach this situation with diplomacy (this is good practice, BTW, for being an engineer whose boss is wrong).

The very first thing you should do is make sure that they're really saying what you think they're saying. I could see that if you're going to be using this Kalman filter as the observer in a control system, then the actual path that you follow won't be the predetermined path with totally random noise -- instead, on average the quadcopter would follow the path. This could be confusing them, or it could be that they want you to include that behavior into your initial simulation and it's confusing you. However, even then I don't think I'd model this behavior the way they're suggesting.

Depending on how rigid they are, and what the prevailing culture is at your University, you either need to find a way to respectfully convince them that you are correct, or you need to find a way to make the whole thing work their way just long enough to get your thesis approved (which -- won't be easy, or good, so unless it's a really nasty environment you want to work on that diplomacy).

Hopefully they're flexible about the approach you take.

The Internet seems to be awash with student theses about guiding quadcopters, as well as various white papers. I would suggest that you do a literature search. What you're looking for is papers that describe successful attempts similar to yours, and that describe the detail you're concerned about. When you do this, be sure that you are open to change -- if all the successful ones do it your profs' way, then you know (and if so do comment on this answer, because I would appreciate the correction!). If you can show your profs that your approach can be a successful one, that should convince them. Make sure you keep notes on where the papers come from, because these will also be valuable to bulk up your citations section when you write your paper.

---

Whew. Back to Technical

My sensors are measuring ... maximum likelihood estimator ... 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 ...

They're both right, or right-ish.

Your approach has the advantage of keeping the core Kalman filter implementation cleaner, but a rangefinding approach to positions isn't linear.

I am assuming that the EKF approach has you modelling a system whose states are the position and whose measurement function gives you the expected sensor ranges from those positions. This will work, and it will automatically take the inherent nonlinearities into account. So -- it's good, and it lets you put "designed and implemented extended Kalman filter" on your resume.

In a larger systems sense, modular is often better, which would make me lean towards your solution if it's good enough. I can see a distinct possibility that your approach could be approximately or even exactly equivalent to the EKF approach, if the output of your position sensor is not only the measured positions, but their predicted noise covariance matrix.

I would:

• Analyze the separate position measurement approach for the actual measurement covariance as a function of position.
• See how much this covariance diverges from a simple diagonal matrix with constant entries on the diagonal.
• Use this as evidence for whether you should worry at all about off-diagonal measurement covariance.

If you do need to take the off-diagonal measurement covariance into account, see if the effect of using an EKF that takes measured ranges instead of a plain ol' KF that takes positions and covariance is substantially different. You will (A) either find out that your 2nd prof is right or (B) have the ammunition to prove to them that you are.

I suspect that if the EKF does have substantially different behavior, it'll only be when the position uncertainty in the filter is large; in this case you have a performance vs. simplicity tradeoff, in that the EKF filter would acquire its states better initially, but the steady-state performance wouldn't be different from the ordinary KF. Depending on the desired behavior of your system this difference in performance could be anywhere from "no one cares" to "that's a vital difference".

(And, in a larger political sense, you may also want to think about putting the magic phrase "extended Kalman filter" onto your resume. If you do the above study and you use the EKF, then when someone asks "so, why an EKF, why not a regular Kalman filter" you can say "because I wanted to put it on my resume, but here's the tradeoff that one could consider if it were a commercial project").

Answer 2

1.- Passive projectiles don't know where they end up

How do you expect to build a 2D model of a random path if, for a passive projectile, as it is now, you are already making certain both start and end positions?

Only the start position should be known, let me explain :

Let's say you are in the range practising with a riffle.

Your model, as it is now, or at least as explained in the question, is kind of :

1.1.- we know where you shooter is

1.2.- we know somehow a hole is already pierced on the target

1.3.- And you are now trying to build a model that adds velocity noise the passive projectile velocity.

Since you are certain that you are going to hit, because it's already hit, what's the point?

2.- Your quadcopter as active projectile

Since the quadcopter could be simplified as an active projectile, meaning it can modify its trajectory, and it has enough intelligence to know where the target is, despite wind, rain and whatever pushes it off course

IF the interference remains below certain levels,

IF the target remains static or moves slow enough

THEN the quadcopter should be bound to arrive to destination, sooner or later, like a 757 travelling between 2 airports : as long as all within specs then, calculating an Estimated Time of Arrival is possible.

You need to discern between when ETA forecast is possible and when it is not.

So, putting aside

• Katryna hurricanes,
• RF jamming,
• target switches on an invisibility cloak,
• target runs away fast enough,
• target swings fast enough to make your bot dance without getting closer,
• quadcopter battery falls flat
• .. else ..

Then the next thing you have to do is

2.1.- build a POINTING VECTOR

2.2.- UPDATE the POINTING VECTOR each cycle or after certain small amount of cycles

2.3.- MAKE the quadcopter MODIFY TRAJECTORY following new POINTING VECTOR.

Now you can even build graphs showing how fast the quadcopter has to travel, for a set of wind speeds, to reach target.