Differences

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

--- cs:hydrophones:multilateration:start [2017/09/14 00:32]
Brian Moore [Multilateration]
+++ cs:hydrophones:multilateration:start [2017/10/07 20:02]
Brian Moore Added Simpler Implementation for Coding
@@ Line 253: / 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