I'm writing a tool whose purpose is to process data from a sensor that provides the true bearing to a target, and combine measurements taken at various times into an estimate of the target's position (at the time of the first measurement) and velocity. The platform bearing the sensor can move in any way while gathering data (and in fact must change velocity to provide a unique solution), but it is assumed that the target travels at a fixed velocity the entire time.
I'm currently using a linear least squares approach to find the solution that minimizes the distance between each bearing line and the point where the target would be at the time of the measurement. The problem is that the distance to a line at bearing 45 degrees is the same as the distance to a line at the reciprocal bearing 45+180 = 225 degrees. This can lead to the target being estimated to be in the opposite direction. I hoped that it wouldn't be a problem in practice, but in fact it is.
My current approach (in detail for those who are interested): for each bearing measurement, we know that $P=(P_x,P_y)$ is the position of the sensor platform, $\mathbf B$ is the normalized bearing line vector, and $t$ is the time since the first measurement. We want to solve for $A=(A_x,A_y)$ and $\mathbf V$, which are the position and velocity of the target at the time of the first measurement.
For each measurement, the target is at $A+\mathbf V t$, and we take $$ε = \operatorname{cross}(\mathbf V)\cdot (A+\mathbf V t -P)$$ (where $\operatorname{cross}\mathbf S$ produces a perpendicular vector to $\mathbf S$, returning $(-S_y,S_x)$. There's probably a name for that.) $A+\mathbf Vt-P$ represents the vector from P to the location of the target for that measurement. We'll call this vector $\mathbf T$. So the dot product gives us $$ε = \cos\theta |\mathbf T | |\operatorname{cross}(\mathbf B)| = \cos\theta |\mathbf T|$$ ($\mathbf B$ is normalized), where θ is the angle between the target and the perpendicular of the bearing line. The cosine function is zero at 90 or 270 degrees, which is why we used the perpendicular vector, so that it would be zero at 0 or 180 degrees to the true bearing line (i.e. it would be zero when |T| coincides with the bearing line). Thus ε is the distance from the estimated target position to the bearing line, and ε is minimized when A and V are such that the estimated target position is on the bearing line.
To solve this, we take ε = cross(B) · (A+Vt-P) = -By(Ax+Vxt-Px) + Bx(Ay+Vyt-Py). Then we square it and take partial derivatives for Ax, Ay, Vx, and Vy, sum the observations, solve the normal equations... the standard least squares approach. (I can elaborate if somebody asks.)
As I said, it's considering the distance to the bearing lines, but I actually want it to consider the distance to the bearing rays. I can't think of a way to do such a thing with linear equations. (In fact, I'm unaware of how to formulate the distance from a point to a ray without using a piecewise function.) I suppose it may not be possible. However, I'm not the best at math, so I ask:
- Is it possible to do with a linear equation?
- If not, can anybody suggest an approach that would work and how to formulate the objective function for it?