|
| 1 | +# Navigation Rebooting |
| 2 | + |
| 3 | +In this problem, we were tasked with determining the position and veolcity of a satellite given psuedorange and velocity data relative to other satellites with known orbits at a fixed point in time. Initially, misunderstanding the difference between a _range_ and a _pseudorange_, I formulated a solution that solved for the least-squares error on the position and velocity while treating the provided measurements as truth. The least-squares solution for position arose from the following derivation: |
| 4 | + |
| 5 | +$$ |
| 6 | +\begin{align*} |
| 7 | +\vec{p} &= \text{position of the satellite we want to solve for} \\ |
| 8 | +\vec{q}&= \text{known position of another satellite} \\ |
| 9 | +\vec{r}&= \text{known position of a second satellite} \\ |
| 10 | +\\ |
| 11 | +\left\Vert \vec{p} - \vec{s} \right\Vert^2 &= \left\Vert \vec{p} \right\Vert^2 - 2 \left( \vec{p} \cdot \vec{s} \right) + \left\Vert \vec{s} \right\Vert^2 & \text{for any satellite $s$} \\ |
| 12 | +\Rightarrow \: \: \left\Vert \vec{p} - \vec{q} \right\Vert^2 - \left\Vert \vec{p} - \vec{r} \right\Vert^2 &= \left( \left\Vert \vec{p} \right\Vert^2 - 2 \left( \vec{p} \cdot \vec{q} \right) + \left\Vert \vec{q} \right\Vert^2 \right) - \left( \left\Vert \vec{p} \right\Vert^2 - 2 \left( \vec{p} \cdot \vec{r} \right) + \left\Vert \vec{r} \right\Vert^2 \right) \\ |
| 13 | +\Rightarrow \: \: \left\Vert \vec{p} - \vec{q} \right\Vert^2 - \left\Vert \vec{p} - \vec{r} \right\Vert^2 - \left\Vert \vec{q} \right\Vert^2 + \left\Vert \vec{r} \right\Vert^2 &= 2 \left( \vec{p} \cdot \vec{r} - \vec{p} \cdot \vec{q} \right) \\ |
| 14 | +\Rightarrow \: \: \left\Vert \vec{p} - \vec{q} \right\Vert^2 - \left\Vert \vec{p} - \vec{r} \right\Vert^2 - \left\Vert \vec{q} \right\Vert^2 + \left\Vert \vec{r} \right\Vert^2 &= 2 p_x (r_x - q_x) + 2 p_y (r_y - q_y) + 2 p_z (r_z - q_z) \\ |
| 15 | +\end{align*} |
| 16 | +$$ |
| 17 | + |
| 18 | +Since all of these values are known except for $p_x$, $p_y$, and $p_z$, we can approximate the solution by the least squares solution to |
| 19 | + |
| 20 | +$$ |
| 21 | +\begin{align*} |
| 22 | +A \begin{bmatrix} p_x \\ p_y \\ p_z \end{bmatrix} &= b \\ |
| 23 | +\\ |
| 24 | +\text{where} \\ |
| 25 | +\\ |
| 26 | +A &= \begin{bmatrix} |
| 27 | +2 (r_{1,x} - q_x) & 2 (r_{1,y} - q_y) & 2 (r_{1,z} - q_z) \\ |
| 28 | +2 (r_{2,x} - q_x) & 2 (r_{2,y} - q_y) & 2 (r_{2,z} - q_z) \\ |
| 29 | +\vdots & \vdots & \vdots \\ |
| 30 | +\end{bmatrix} \\ |
| 31 | +b &= \begin{bmatrix} |
| 32 | +\left\Vert \vec{p} - \vec{q} \right\Vert^2 - \left\Vert \vec{p} - \vec{r_1} \right\Vert^2 - \left\Vert \vec{q} \right\Vert^2 + \left\Vert \vec{r_1} \right\Vert^2 \\ |
| 33 | +\left\Vert \vec{p} - \vec{q} \right\Vert^2 - \left\Vert \vec{p} - \vec{r_2} \right\Vert^2 - \left\Vert \vec{q} \right\Vert^2 + \left\Vert \vec{r_2} \right\Vert^2 \\ |
| 34 | +\vdots |
| 35 | +\end{bmatrix} |
| 36 | +\end{align*} |
| 37 | +$$ |
| 38 | + |
| 39 | +by picking one of the satellites with known orbit to act as $q$ and using the other satellites as the various values of $r_i$. |
| 40 | + |
| 41 | +For velocity, we used the following derivation: |
| 42 | + |
| 43 | +$$ |
| 44 | +\begin{align*} |
| 45 | +v_{pq} &= \text{rate of change of distance from $p$ to $q$ (known)} \\ |
| 46 | +\vec{v_p} &= \text{velocity of $p$ (unknown)} \\ |
| 47 | +\vec{v_q} &= \text{velocity of $q$ (known)} \\ |
| 48 | +\hat{pq} &= \text{unit vector from $p$ to $q$ (known)} \\ |
| 49 | +\\ |
| 50 | +v_{pq} &= \left( \vec{v_q} - \vec{v_p} \right) \cdot \hat{pq} \\ |
| 51 | +\Rightarrow \: \: v_{pq} &= \vec{v_q} \cdot \hat{pq} - \vec{v_p} \cdot \hat{pq} \\ |
| 52 | +\Rightarrow \: \: \vec{v_p} \cdot \hat{pq} &= \vec{v_q} \cdot \hat{pq} - v_{pq} |
| 53 | +\end{align*} |
| 54 | +$$ |
| 55 | + |
| 56 | +Since the only unknown is $\vec{v_p}$, we can solve for it by finding the least-squares solution to |
| 57 | + |
| 58 | +$$ |
| 59 | +\begin{align*} |
| 60 | +A \begin{bmatrix} {v_p}_x \\ {v_p}_y \\ {v_p}_z \end{bmatrix} &= b \\ |
| 61 | +\\ |
| 62 | +\text{where} \\ |
| 63 | +\\ |
| 64 | +A &= \begin{bmatrix} |
| 65 | +\hat{pq_1}_x & \hat{pq_1}_y & \hat{pq_1}_z \\ |
| 66 | +\hat{pq_2}_x & \hat{pq_2}_y & \hat{pq_2}_z \\ |
| 67 | +\vdots & \vdots & \vdots \\ |
| 68 | +\end{bmatrix} \\ |
| 69 | +b &= \begin{bmatrix} |
| 70 | +\vec{v_{q_1}} \cdot \hat{pq_1} - v_{pq_1} \\ |
| 71 | +\vec{v_{q_2}} \cdot \hat{pq_2} - v_{pq_2} \\ |
| 72 | +\vdots |
| 73 | +\end{bmatrix} |
| 74 | +\end{align*} |
| 75 | +$$ |
| 76 | + |
| 77 | +using the known satellites for the various values of $q_i$. |
| 78 | + |
| 79 | +Running this solution got us _close_, but not close enough. The reason for this is that we were given _pseudoranges_ rather than ranges, the difference being that ranges are treated as absolute distance measurements while pseudoranges may have errors caused by receiver clock bias and path delays. |
| 80 | + |
| 81 | +To solve using the pseudoranges, I implemented the algorithm described [in this helpful summary of the topic](http://www.grapenthin.org/notes/2019_03_11_pseudorange_position_estimation/); see the implementation in `solve_pseudo.py`. Once you work through all of the math, it boils down to iterating on this linear system until you sufficiently converge on a solution: |
| 82 | + |
| 83 | +$$ |
| 84 | +\begin{align*} |
| 85 | +\vec{p} &= \text{our current solution for the position of the satellite} \\ |
| 86 | +t &= \text{our current solution for the clock bias} \\ |
| 87 | +\vec{s_i} &= \text{known position of satellite $i$ at time of observation} \\ |
| 88 | +\rho_i &= \text{observed distance to satellite $i$} \\ |
| 89 | +c &= \text{the speed of light} \\ |
| 90 | +\ell &= \text{gradient rate (we used 0.01)} |
| 91 | +\\ |
| 92 | +\\ |
| 93 | +\text{Solve} \; \; A \begin{bmatrix}\Delta p_x \\ \Delta p_y \\ \Delta p_z \\ \Delta t\end{bmatrix} &= b \\ |
| 94 | +\text{where} \; \; A &= \begin{bmatrix} |
| 95 | + \frac{{s_1}_x}{\left\Vert \vec{p} - \vec{s_1} \right\Vert} & |
| 96 | + \frac{{s_1}_y}{\left\Vert \vec{p} - \vec{s_1} \right\Vert} & |
| 97 | + \frac{{s_1}_z}{\left\Vert \vec{p} - \vec{s_1} \right\Vert} & |
| 98 | + c \\ |
| 99 | + \vdots & \vdots & \vdots & \vdots |
| 100 | +\end{bmatrix} \\ |
| 101 | +b &= \begin{bmatrix} |
| 102 | + \rho_1 - \left\Vert \vec{p} - \vec{s_1} \right\Vert - c t \\ |
| 103 | + \vdots |
| 104 | +\end{bmatrix} \\ |
| 105 | +\\ |
| 106 | +\\ |
| 107 | +\text{and then update} \; \; \vec{p'} &= \vec{p} + \ell \begin{bmatrix}\Delta p_x \\ \Delta p_y \\ \Delta p_z\end{bmatrix} \\ |
| 108 | +t' &= t + \ell \Delta t |
| 109 | +\end{align*} |
| 110 | +$$ |
| 111 | + |
| 112 | +Note that this solves only for position and not for velocity. |
| 113 | + |
| 114 | +We tried submitting this solution with the best velocity value found from our old solver, but this wasn't close enough. However, we noticed that the solution that our pseudorange solver converged upon was consistent with almost exactly 50 microseconds of clock bias, or 15km of distance error. By hardcoding this 15km into the original solver, we were able to produce a solution that was close enough to get a flag; see `solve.py`, and note the hardcoded value on line 85. |
0 commit comments