This shows you the differences between two versions of the page.
Both sides previous revision Previous revision Next revision | Previous revision Last revision Both sides next revision | ||
cs:hydrophones:trilateration_setup:start [2016/11/15 19:09] James Irwin [Solving for Position] |
cs:hydrophones:trilateration_setup:start [2017/09/12 12:42] James Irwin |
||
---|---|---|---|
Line 1: | Line 1: | ||
- | ====== Trilateration ====== | + | ====== Trilateration Setup====== |
Below is the math for calculating the location of the pinger in the water relative to our submarine. Because we know the absolute location of the pinger in the pool, we can calculate the sub's position in the pool. | Below is the math for calculating the location of the pinger in the water relative to our submarine. Because we know the absolute location of the pinger in the pool, we can calculate the sub's position in the pool. | ||
Line 20: | Line 20: | ||
$$ | $$ | ||
- | When a ping is received by the hydrophones, the hardware outputs delta-timestamps $\Delta t_x$, $\Delta t_y$, $\Delta t_z$, which corresponds to the difference in time between the ping was received by $h_0$ and $h_{x,y,z}$, respectively. | + | When a ping is received by the hydrophones, the hardware outputs delta-timestamps $\Delta t_x$, $\Delta t_y$, $\Delta t_z$, which corresponds to the difference in time between when the ping was received by $h_0$ and $h_{x,y,z}$, respectively. |
Let's define $p_0$ as the absolute distance between $h_0$ and the pinger at location $(x,y,z)$. | Let's define $p_0$ as the absolute distance between $h_0$ and the pinger at location $(x,y,z)$. | ||
Line 33: | Line 33: | ||
\Delta z = \Delta t_z * c_s \\ | \Delta z = \Delta t_z * c_s \\ | ||
$$ | $$ | ||
- | In other words, $h_x$ is $\Delta x$ meters farther from the pinger than $h_0$, and $h_0$ is $p_0$ meters from the pinger. | + | In other words, $h_x$ is $\Delta x$ meters closer to the pinger than $h_0$, and $h_0$ is $p_0$ meters from the pinger. |
The final calculations for $x$, $y$, and $z$ will be in terms of $\Delta x$, $\Delta y$, and $\Delta z$ | The final calculations for $x$, $y$, and $z$ will be in terms of $\Delta x$, $\Delta y$, and $\Delta z$ | ||
Line 141: | Line 141: | ||
x^2 = p_0^2 \frac{4\Delta x^2}{4 \delta^2} + p_0 \frac{4\Delta x}{4\delta^2}(-\Delta x^2 + \delta^2) + \frac{(\Delta x^2 - \delta^2)^2}{4 \delta^2} \\ | x^2 = p_0^2 \frac{4\Delta x^2}{4 \delta^2} + p_0 \frac{4\Delta x}{4\delta^2}(-\Delta x^2 + \delta^2) + \frac{(\Delta x^2 - \delta^2)^2}{4 \delta^2} \\ | ||
- | x^2 = p_0^2 \left(\frac{\Delta x}{\delta}\right)^2 + p_0 \frac{\Delta x}{\delta^2}(\delta^2 +\Delta x^2) + \left(\frac{\Delta x^2 - \delta^2}{2 \delta}\right)^2 \\ | + | x^2 = p_0^2 \left(\frac{\Delta x}{\delta}\right)^2 + p_0 \frac{\Delta x}{\delta^2}(\delta^2 -\Delta x^2) + \left(\frac{\Delta x^2 - \delta^2}{2 \delta}\right)^2 \\ |
$$ | $$ | ||
We can derive a similar equation for $y^2$ and $z^2$ | We can derive a similar equation for $y^2$ and $z^2$ | ||
$$ | $$ | ||
- | x^2 = p_0^2 \left(\frac{\Delta x}{\delta}\right)^2 + p_0 \frac{\Delta x}{\delta^2}(\delta^2 +\Delta x^2) + \left(\frac{\Delta x^2 - \delta^2}{2 \delta}\right)^2 \\ | + | x^2 = p_0^2 \left(\frac{\Delta x}{\delta}\right)^2 + p_0 \frac{\Delta x}{\delta^2}(\delta^2 -\Delta x^2) + \left(\frac{\Delta x^2 - \delta^2}{2 \delta}\right)^2 \\ |
- | y^2 = p_0^2 \left(\frac{\Delta y}{\epsilon}\right)^2 + p_0 \frac{\Delta y}{\epsilon^2}(\epsilon^2 +\Delta y^2) + \left(\frac{\Delta y^2 - \epsilon^2}{2 \epsilon}\right)^2 \\ | + | y^2 = p_0^2 \left(\frac{\Delta y}{\epsilon}\right)^2 + p_0 \frac{\Delta y}{\epsilon^2}(\epsilon^2 -\Delta y^2) + \left(\frac{\Delta y^2 - \epsilon^2}{2 \epsilon}\right)^2 \\ |
- | z^2 = p_0^2 \left(\frac{\Delta z}{\zeta}\right)^2 + p_0 \frac{\Delta z}{\zeta^2}(\zeta^2 +\Delta z^2) + \left(\frac{\Delta z^2 - \zeta^2}{2 \zeta}\right)^2 \\ | + | z^2 = p_0^2 \left(\frac{\Delta z}{\zeta}\right)^2 + p_0 \frac{\Delta z}{\zeta^2}(\zeta^2 -\Delta z^2) + \left(\frac{\Delta z^2 - \zeta^2}{2 \zeta}\right)^2 \\ |
$$ | $$ | ||
Line 161: | Line 161: | ||
$$ | $$ | ||
a_x = \left(\frac{\Delta x}{\delta}\right)^2 \qquad | a_x = \left(\frac{\Delta x}{\delta}\right)^2 \qquad | ||
- | b_x = \frac{\Delta x}{\delta^2}(\delta^2 +\Delta x^2) \qquad | + | b_x = \frac{\Delta x}{\delta^2}(\delta^2 -\Delta x^2) \qquad |
c_x = \left(\frac{\Delta x^2 - \delta^2}{2 \delta}\right)^2 \\ | c_x = \left(\frac{\Delta x^2 - \delta^2}{2 \delta}\right)^2 \\ | ||
a_y = \left(\frac{\Delta y}{\epsilon}\right)^2 \qquad | a_y = \left(\frac{\Delta y}{\epsilon}\right)^2 \qquad | ||
- | b_y = \frac{\Delta y}{\epsilon^2}(\epsilon^2 +\Delta y^2) \qquad | + | b_y = \frac{\Delta y}{\epsilon^2}(\epsilon^2 -\Delta y^2) \qquad |
c_y = \left(\frac{\Delta y^2 - \epsilon^2}{2 \epsilon}\right)^2 \\ | c_y = \left(\frac{\Delta y^2 - \epsilon^2}{2 \epsilon}\right)^2 \\ | ||
a_z = \left(\frac{\Delta z}{\zeta}\right)^2 \qquad | a_z = \left(\frac{\Delta z}{\zeta}\right)^2 \qquad | ||
- | b_z = \frac{\Delta z}{\zeta^2}(\zeta^2 +\Delta z^2) \qquad | + | b_z = \frac{\Delta z}{\zeta^2}(\zeta^2 -\Delta z^2) \qquad |
c_z = \left(\frac{\Delta z^2 - \zeta^2}{2 \zeta}\right)^2 | c_z = \left(\frac{\Delta z^2 - \zeta^2}{2 \zeta}\right)^2 | ||
$$ | $$ | ||
Line 185: | Line 185: | ||
\begin{equation} \label{eq:p_0_from_position} | \begin{equation} \label{eq:p_0_from_position} | ||
p_0^2 = x^2 + y^z + z^2 | p_0^2 = x^2 + y^z + z^2 | ||
- | \end equation | + | \end{equation} |
$$ | $$ | ||
Combining equations $\ref{eq:squared_pos_from_p0_simplified}$ and $\ref{eq:p_0_from_position}$, we get | Combining equations $\ref{eq:squared_pos_from_p0_simplified}$ and $\ref{eq:p_0_from_position}$, we get | ||
Line 194: | Line 194: | ||
$$ | $$ | ||
- | Letting $a = a_x + a_y + a_z$ (do the same for $b$ and $c$), we can simplify to | + | Letting |
+ | $$ | ||
+ | a = a_x + a_y + a_z - 1 \\ | ||
+ | b = b_x + b_y + b_z \\ | ||
+ | c = c_x + c_y + c_z | ||
+ | $$ | ||
+ | we can simplify to | ||
$$ | $$ | ||
\begin{equation} \label{eq:p_0_final} | \begin{equation} \label{eq:p_0_final} |