User Tools


Differences

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

Link to this comparison view

Both sides previous revision Previous revision
Next revision
Previous revision
cs:hydrophones:multilateration:start [2017/09/14 00:13]
Brian Moore
cs:hydrophones:multilateration:start [2017/10/07 20:04] (current)
Brian Moore [Coding Implementation]
Line 1: Line 1:
 ====== Multilateration====== ====== 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 precise ​calculation the evaluates at all distancesand 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. 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)$. ​ 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}$. 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) \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 cumbersome for a real implementation. ​ Because the results are all 2x2 determinates with common 2nd rows, we can simplify the calculation significantly. ​ As such, the following implementation may be preferable:
 +
 +$$
 +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
 +2
 +\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
 +2
 +\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}$. ​