Differences

This shows you the differences between two versions of the page.

--- cs:hydrophones:multilateration:start [2017/09/14 00:13]
Brian Moore
+++ cs:hydrophones:multilateration:start [2017/10/07 20:02]
Brian Moore Added Simpler Implementation for Coding
@@ Line 1: / Line 1: @@
 ====== Multilateration======
-Previous derivations on this topic were made in years passed to enable calculations with limited numbers of hydrophone channels.  This page provides a generalized algorithm for locating a pinger in 3D in a robust, linear manner with an arbitrary number of arbitrarily placed hydrophones.  This solution is a precise calculation the evaluates at all distances, and does not require require that the signal originate in the far-field.
+Previous derivations on this topic were made in years passed to enable calculations with limited numbers of hydrophone channels.  This page provides a generalized algorithm for locating a pinger in 3D in a robust, linear manner with an arbitrary number of arbitrarily placed hydrophones.  This solution is an unambiguous, precise result that evaluates at all distances and does not require that the signal originate in the far-field.
 This method of calculation requires 5 or more hydrophones.  Substituting a depth sensor for one of the required hydrophones is possible, and covered below.  The submarine does //not// need to be held level to make use of the depth sensor.
@@ Line 144: / Line 144: @@
 In the event the submarine is rotated off of the default orientation $R(\psi,\phi,\theta) = R(0,0,0)$ then the relative estimation of the pinger's position will be off, and a correction must be made to get the absolute coordinates $p_{pinger}=(X,Y,Z)$.
-If the array is rotated by $R$ then the relative position of the pinger has been rotated in the local frame by $R^T$
+If the array is rotated by $R$ then the relative position of the pinger has been rotated in the local frame by the inverse $R^T$
 $$
@@ Line 173: / Line 173: @@
 $$
-Which requires only 4 hydrophones, $h_0,h_1,h_2,$ and $h_3$ plus the depth sensor measuring $z_{sub}$ to compared against the known depth of the pinger $z_pinger$.
+Which requires only 4 hydrophones, $h_0,h_1,h_2,$ and $h_3$ plus the depth sensor measuring $z_{sub}$ to compared against the known depth of the pinger $z_{pinger}$.
 However, the depth sensor will always measure the absolute depth of the submarine regardless of the submarine's orientation $R(\psi,\phi,\theta)$.  Thus the difference measured will always be $z_{pinger}-z_{sub} = Z_{global}$.
@@ Line 225: / Line 225: @@
 \right)
 \\
+$$
+With polar bearings:
+$$
+Azimuth = arctan\left(\frac{Y}{X}\right), \qquad
+Inclination = arctan\left(\frac{Z}{\sqrt{X^2+Y^2}}\right), \qquad
+Range = \sqrt{X^2 + Y^2 + Z^2} \\
 $$
@@ Line 244: / Line 253: @@
 $$
+===== Coding Implementation =====
+The elements of the linear equations all being determinates is enumerated as such to maintain the underlying structure of the calculations.  However calculating all those determinates is unnecessarily cumbersome for a real implementation.  Because the results are all 2x2 determinates with common 2nd rows, we can simplify the calculation significantly.
+$$
+AR^TX_{global} = C - B \\
+A =
+\begin{bmatrix}
+\begin{vmatrix} 2x_1 & 2x_2 \\ \Delta r_1 & \Delta r_2 \\ \end{vmatrix},
+&
+\begin{vmatrix} 2y_1 & 2y_2 \\ \Delta r_1 & \Delta r_2 \\ \end{vmatrix},
+&
+\begin{vmatrix} 2z_1 & 2z_2 \\ \Delta r_1 & \Delta r_2 \\ \end{vmatrix}
+\\
+\begin{vmatrix} 2x_1 & 2x_3 \\ \Delta r_1 & \Delta r_3 \\ \end{vmatrix},
+&
+\begin{vmatrix} 2y_1 & 2y_3 \\ \Delta r_1 & \Delta r_3 \\ \end{vmatrix},
+&
+\begin{vmatrix} 2z_1 & 2z_3 \\ \Delta r_1 & \Delta r_3 \\ \end{vmatrix}
+\\
+\begin{vmatrix} 2x_1 & 2x_4 \\ \Delta r_1 & \Delta r_4 \\ \end{vmatrix},
+&
+\begin{vmatrix} 2y_1 & 2y_4 \\ \Delta r_1 & \Delta r_4 \\ \end{vmatrix},
+&
+\begin{vmatrix} 2z_1 & 2z_4 \\ \Delta r_1 & \Delta r_4 \\ \end{vmatrix}
+\end{bmatrix}
+=
+\begin{bmatrix}
+\Delta r_2 & -\Delta r_1 & 0 & 0 \\
+\Delta r_3 & 0 & -\Delta r_1 & 0 \\
+\Delta r_4 & 0 & 0 & -\Delta r_1
+\end{bmatrix}
+\cdot
+\cdot
+\begin{bmatrix}
+x_1 & y_1 & z_1 \\
+x_2 & y_2 & z_2 \\
+x_3 & y_3 & z_3 \\
+x_4 & y_4 & z_4
+\end{bmatrix}
+\\
+C =
+\begin{bmatrix}
+\begin{vmatrix} (x_1^2+y_1^2+z_1^2) & (x_2^2+y_2^2+z_2^2) \\ \Delta r_1 & \Delta r_2 \\ \end{vmatrix} \\
+\begin{vmatrix} (x_1^2+y_1^2+z_1^2) & (x_3^2+y_3^2+z_3^2) \\ \Delta r_1 & \Delta r_3 \\ \end{vmatrix} \\
+\begin{vmatrix} (x_1^2+y_1^2+z_1^2) & (x_4^2+y_4^2+z_4^2) \\ \Delta r_1 & \Delta r_4 \\ \end{vmatrix} \\
+\end{bmatrix}
+=
+\begin{bmatrix}
+\Delta r_2 & -\Delta r_1 & 0 & 0 \\
+\Delta r_3 & 0 & -\Delta r_1 & 0 \\
+\Delta r_4 & 0 & 0 & -\Delta r_1
+\end{bmatrix}
+\begin{bmatrix}
+(x_1^2+y_1^2+z_1^2) \\
+(x_2^2+y_2^2+z_2^2) \\
+(x_3^2+y_3^2+z_3^2) \\
+(x_4^2+y_4^2+z_4^2)
+\end{bmatrix}
+\\
+B =
+\begin{bmatrix}
+\begin{vmatrix} \Delta r_1^2 & \Delta r_2^2 \\ \Delta r_1 & \Delta r_2 \\ \end{vmatrix} \\
+\begin{vmatrix} \Delta r_1^2 & \Delta r_3^2 \\ \Delta r_1 & \Delta r_3 \\ \end{vmatrix} \\
+\begin{vmatrix} \Delta r_1^2 & \Delta r_4^2 \\ \Delta r_1 & \Delta r_4 \\ \end{vmatrix} \\
+\end{bmatrix}
+=
+\begin{bmatrix}
+\Delta r_2 & -\Delta r_1 & 0 & 0 \\
+\Delta r_3 & 0 & -\Delta r_1 & 0 \\
+\Delta r_4 & 0 & 0 & -\Delta r_1
+\end{bmatrix}
+\begin{bmatrix}
+\Delta r_1^2 \\
+\Delta r_2^2 \\
+\Delta r_3^2 \\
+\Delta r_4^2
+\end{bmatrix}
+$$
+We've taken the 2x2 determinate operation and transformed it into a matrix product.  Now all the elements more clearly become the result of the measurements and the position of the hydrophones:
+$$
+\begin{bmatrix}
+\Delta r_2 & -\Delta r_1 & 0 & 0 \\
+\Delta r_3 & 0 & -\Delta r_1 & 0 \\
+\Delta r_4 & 0 & 0 & -\Delta r_1
+\end{bmatrix}
+\cdot
+\cdot
+\begin{bmatrix}
+x_1 & y_1 & z_1 \\
+x_2 & y_2 & z_2 \\
+x_3 & y_3 & z_3 \\
+x_4 & y_4 & z_4
+\end{bmatrix}
+\begin{bmatrix}
+R
+\end{bmatrix}^T
+\begin{bmatrix}
+X \\
+Y \\
+Z
+\end{bmatrix}
+=
+\begin{bmatrix}
+\Delta r_2 & -\Delta r_1 & 0 & 0 \\
+\Delta r_3 & 0 & -\Delta r_1 & 0 \\
+\Delta r_4 & 0 & 0 & -\Delta r_1
+\end{bmatrix}
+\left(
+\begin{bmatrix}
+(x_1^2+y_1^2+z_1^2) \\
+(x_2^2+y_2^2+z_2^2) \\
+(x_3^2+y_3^2+z_3^2) \\
+(x_4^2+y_4^2+z_4^2)
+\end{bmatrix}
+-
+\begin{bmatrix}
+\Delta r_1^2 \\
+\Delta r_2^2 \\
+\Delta r_3^2 \\
+\Delta r_4^2
+\end{bmatrix}
+\right)
+\\
+r_{det} =
+\begin{bmatrix}
+\Delta r_2 & -\Delta r_1 & 0 & 0 \\
+\Delta r_3 & 0 & -\Delta r_1 & 0 \\
+\Delta r_4 & 0 & 0 & -\Delta r_1
+\end{bmatrix},
+\quad
+H =
+\begin{bmatrix}
+x_1 & y_1 & z_1 \\
+x_2 & y_2 & z_2 \\
+x_3 & y_3 & z_3 \\
+x_4 & y_4 & z_4
+\end{bmatrix}
+\\
+r_{det} \cdot 2HR^TX_{global} = r_{det} \cdot (|H|^2 - \Delta r^2) \\
+X_{global} = inv(r_{det} \cdot 2HR^T) \cdot r_{det} \cdot (|H|^2 - \Delta r^2)
+$$
+Which is a much simpler implementation.  Only $r_{det}$ and $\Delta r^2$ need to be re-calculated upon every measurement.
+A few notes:
+- $r_{det}$ cannot be factored out of the equation because it is not a full-rank invertible matrix.
+- The role of rotation matrix $R(\psi,\phi,\theta)$ is much more evident here. The submarine being rotated off of alignment with the global coordinate space is equivalent to the positions of all the hydrophones $H$ being rotated by the same amount.
+- When mixing in the depth sensor in place of your fourth non-reference hydrophone $h_4$, remove the 4th row from $H$ and $\Delta r^2$, as well as the third row and fourth column from $r_{det}$.

User Tools

Differences

Page Tools

Palouse RoboSub Technical Documentation

Site Tools