This shows you the differences between two versions of the page.
Both sides previous revision Previous revision Next revision | Previous revision | ||
cs:hydrophones:multilateration:start [2017/09/14 00:32] Brian Moore [Multilateration] |
cs:hydrophones:multilateration:start [2017/10/07 20:04] (current) Brian Moore [Coding Implementation] |
||
---|---|---|---|
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 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}$. | ||