linear least squares minimizing distance from points to rays - is it possible?

Question

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) = -B_y(A_x+V_xt-P_x) + B_x(A_y+V_yt-P_y). Then we square it and take partial derivatives for A_x, A_y, V_x, and V_y, 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:

1. Is it possible to do with a linear equation?
2. If not, can anybody suggest an approach that would work and how to formulate the objective function for it?

Answer 1

ad 1) Because any linear function of T can be written in the form ε = N · T for some vector N, there is little hope to improve your method if we are restricted to linear least squares.

EDIT It's actually possible to formulate it as a linear least squares problem with linear restrictions, which allows to see that just computing the distance from the ray as you proposed should work fine and is easy to implement, see (*).

So better don't try to implement what I suggested under "ad 2)", because it is more complicated than necessary.

ad 2) Formulate it as a nonlinear least squares problem. Both the Gauss-Newton algorithm (GNA) and the Levenberg–Marquardt algorithm (LMA) are well suited to solve this type of problem. There are many free implementations of LMA available. It may be a good idea to generate a sufficient large number of random initial values to avoid problems with local minima or non-convergence.

I would compute the objective functions as follows: For each bearing measurement, apply a rotation matrix $R$ to the analytic expression for T such that the positive x-axis becomes the bearing ray. Then use ε = atan2(y-coord,x-coord) as measurement error whose sum of squares should be minimized.

A problem with this approach can occur when the target passes closely by the sensor, because the errors introduced by the assumption that the target travels at constant speed is not modeled well (and because atan2 has a singularity at (0,0) which was not there for your linear approach). This can be fixed by treating T for each bearing measurement as unknown, and minimizing it's deviation from the analytic expression for T. However, the random initial values should only be generated for A and V, and the initial values for T just computed with the analytic expression. (If this should really lead to convergence problems, then the initial values for T could also be randomly perturbed a bit.)

EDIT(*) When we treat T as unknown, we actually have to decide how we want to weight the errors of the measurements and the errors of our assumption against each other. In case we assume that the errors of the bearing measurements are negligible with respect to the assumption of constant velocity, we basically arrive again at the initial linear least squares formulation. It now reads $0$ = cross(B) · T and $0\leq$ B · T together with the fact that |T - (A+Vt-P)| should be minimized. This is identical to the initial formulation in case $0<$ B · T. Otherwise, we have to replace the distance from the line with the distance from the origin. (Which is equivalent to your initial idea to use the distance from the ray for the objective function.) The theory of quadratic programming has more to say about how the switching between the different cases has to be done correctly, but I hope it shouldn't be too difficult for this specific case.

EDIT 2 The squared distance from the bearing ray could be written in the form $[cross(B)\cdot T]^2 + [\max(0,-B\cdot T)]^2$. I would probably use this together with my favorite Levenberg-Marquardt implementation, because then I don't have to worry about solving the normal equations myself. If I would instead solve the "local" normal equations until the "local" normal equations no longer change, this would be equivalent to using the Gauss-Newton method. I'm just not sure whether this is guaranteed to work, that's why I pointed to the quadratic programming solution (which is guaranteed to work).

Answer 2

Note: This is extracted from a document I wrote about 20 years ago. My LaTeX is a little rusty now, so the formatting is not quite what it should be. Feel free to make it look better. $$\def\\{\backslash } \def\xmean#1{< \negthinspace #1 \negthinspace > } \def\mean#1{\hbox{$ \xmean{#1} $}} \def\cov#1{\hbox{$ \xmean{\negthinspace\xmean{#1}\negthinspace} $}} \def\hq{\ } \def\q{\quad} \def\qq{\q\q} \def\qqq{\qq\q} \def\qqqq{\qqq\q}$$ The problem investigated here is that of fitting a straight line to a set of data points. The fit is intended to be independent of the coordinate system in the sense that, if the data points are rotated or shifted, the resulting line will be the same relative to the points. The standard least squares fitted line does not satisfy this criteria, because the error in the fit for each point is considered to be the distance from the point to the line parallel to one of the ordinate axes. When the axes are rotated, the distance changes. We choose to use the actual distance from the point to the line as the error in the fit. This yields an error which is invariant under translations and rotations.

The form of the equation of a straight line most useful for our purpose is the $polar$ form. In this form, a line $L$ is determined by its distance r from the origin and the angle $\theta$ that the normal from the origin to the line makes with the $x$ axis, the angle being measured counterclockwise. The equation of $L$ in terms of r and $\theta$ is $$L:~~ x\cos \theta + y \sin \theta ~=~r.$$ The polar form for the equation of $L$ is so useful because the distance from any point $(u, v)$ to $L$ is $u\cos \theta + v \sin \theta - r$.

We now define some notation and abbreviations. Let $c~=~\cos \theta$ and $s~=~\sin \theta$, so the equation of $L$ is $cx+sy~=~r$. The data points used to fit $L$ are $(x_i, y_i)$ for $i=1$ to $n$ (i.e., there are $n$ points). For any expression f, we define $\mean{f} $ to be the average of f over all the data points, so that $$\mean{f} ~=~(1/n)\sum_{i=1}^n f_i.$$ For example, $\mean{x} ~=~(1/n)\sum_{i=1}^n x_i$, and $\mean{xy} ~=~(1/n)\sum_{i=1}^n x_i y_i$. We could also define $\mean{f} $ to be a weighted mean and the results which follow would not be affected at all.

If $p$ and $q$ are any expressions, we define $$\cov{p,q}~=~\mean{pq} -\mean{p} \mean{q},$$ the covariance between $p$ and $q$. For example, $$\cov{x,x}~=~\mean{x^2}-\mean{x}^2 \hq{\rm and}\hq \cov{x,y}~=~\mean{xy}-\mean{x}\mean{y}.$$

We first derive an expression for the mean squared error of the fit of $L$ to the data points. If D is this error, then $$\eqalignno{ D &~=~ \mean{({\rm distance\;from\;point\;}i{\rm\;to\;}L)^2} \cr &~=~ \mean{(cx+sy-r)^2} \cr &~=~ \mean{c^2x^2+s^2y^2+r^2 + 2csxy-2crx-2sry} \cr &~=~ c^2\mean{x^2} +s^2\mean{y^2} +r^2+ 2sc\mean{xy} -2cr\mean{x} -2sr\mean{y}.\cr }$$

If $L$ is to be the best fitting line in the least mean squared sense, we must have $\partial D / \partial r=0$ and $\partial D / \partial \theta=0$. However, the values of $r$ and $\theta$ that minimize $D$ can be found without using any calculus. This will now be done by writing $D$ as the sum of terms which, when independently minimized, give the desired values for $r$ and $\theta$. $$\eqalignno{ D~&=~ c^2\mean{x^2} +s^2\mean{y^2} +2sc\mean{xy} +r^2 -2r(c\mean{x} +s\mean{y})\cr &=~ c^2\mean{x^2} +s^2\mean{y^2} +2sc\mean{xy} +(r-c\mean{x} -s\mean{y})^2 -(c\mean{x} +s\mean{y})^2\cr &=~c^2(\mean{x^2}-\mean{x}^2) + s^2(\mean{y^2}-\mean{y}^2) + 2sc(\mean{xy}-\mean{x}\mean{y}) +(r-c\mean{x} -s\mean{y})^2\cr &=~c^2\cov{x,x} + s^2\cov{y,y}+2sc\cov{x,y} +(r-c\mean{x} -s\mean{y})^2.\cr }$$ Letting $S=\sin 2\theta =2sc$ and $C=\cos 2\theta=c^2-s^2$, since $c^2=(1+C)/2$ and $s^2=(1-C)/2$, $$\eqalignno{ D~&=~{\cov{x,x}+\cov{y,y} \over 2} + C {\cov{x,x}-\cov{x,y} \over 2} + S \cov{x,y} +(r-c\mean{x} -s\mean{y})^2\cr &=~D_1+C~D_2+S~D_3 +(r-c\mean{x} -s\mean{y})^2\cr }$$ where $$D_1={\cov{x,x} +\cov{y,y} \over 2},\hq D_2={\cov{x,x} -\cov{y,y} \over 2},\hq {\rm and}\hq D_3=\cov{x,y}.$$ Defining $R$ and $\phi$ by $D_2=R\cos\phi$ and $D_3=R\sin\phi$, where $R \ge 0$ and $0 \le \phi < 2\pi$, $$\eqalignno{ D~&=~D_1+R\cos 2\theta\cos\phi+R\sin 2\theta\sin\phi +(r-c\mean{x} -s\mean{y})^2\cr &=~D_1+R\cos(2\theta-\phi) +(r-c\mean{x} -s\mean{y})^2.\cr }$$ This is the desired expression for $D$.

Since $\cos(2\theta-\phi) \ge -1$ and $(r-c\mean{x}-s\mean{y})^2 \ge 0$, $D \ge D_1-R$. By first choosing $\theta=(\phi+\pi)/2$ and then setting $r=\mean{x}\cos\theta + \mean{y}\sin\theta$, $D$ will achieve its minimum value of $D_1 - R$.