# An Iterative Guidance and Navigation Algorithm for Orbit Rendezvous of Cooperating CubeSats
## 3 Guidance and Navigation
### Guidance  
This work assumes that both the chaser and the target travel orbits sufficiently close to the reference circular Keplerian orbit. This implies that the orbital dynamics of both vehicles can be described by using the HCW Equation (4). Let \(\delta\mathbf{r}:=[x\quad y\quad z]\) and \(\delta\mathbf{v}:=[\dot{x}\quad\dot{y}\quad\dot{z}]\). Due to the linear nature of the governing Equation (4), the time evolution of the displaced position and velocity vectors \(\delta\mathbf{r}(t)\) and \(\delta\mathbf{v}(t)\) is given by [14]  
\[\left[\begin{array}{c}\delta\mathbf{r}(t)\\ \delta\mathbf{v}(t)\end{array}\right]=\left[\begin{array}{cc}M(\tau)&N(\tau) \\ S(\tau)&T(\tau)\end{array}\right]\left[\begin{array}{c}\delta\mathbf{r}(t_{k })\\ \delta\mathbf{v}(t_{k})\end{array}\right], \tag{5}\]  
where \(\tau=t-t_{k}\). The closed-form expressions of the \((3\times 3)\) matrices \(M\), \(N\), \(S\), and \(T\) are reported in [14].  
The rendezvous maneuver is assumed to have a specified duration \(\Delta t_{tot}\). At a generic time \(t_{k}\) (\(0\leq t_{k}\leq\Delta t_{tot}\)), the maneuver at hand can be designed by using Equation (4), by including a pair of impulsive changes of velocity, the first at time \(t_{k}\) and the second at time \(\Delta t_{tot}\). Superscripts + and \(-\) are associated with the time instants immediately before and after each velocity changes, i.e.,  
\[\left\{\begin{array}{l}\Delta\mathbf{v}_{1}=\delta\mathbf{v}(t_{k}^{+})- \delta\mathbf{v}(t_{k}^{-})\\ \Delta\mathbf{v}_{2}=\delta\mathbf{v}(\Delta t_{tot}^{+})-\delta\mathbf{v}( \Delta t_{tot}^{-})\end{array}\right.. \tag{6}\]  
The final goal is to get the position and velocity of the target at time \(\Delta t_{tot}\), i.e.,  
\[\left\{\begin{array}{l}\delta\mathbf{r}(\Delta t_{tot})=\Delta\mathbf{r}_{ T}(t_{tot})\\ \delta\mathbf{v}(\Delta t_{tot}^{+})=\Delta\mathbf{v}_{T}(t_{tot})\end{array} \right., \tag{7}\]  
where subscript T refers to the target. The magnitudes and directions of \(\Delta\mathbf{v}_{1}\) and \(\Delta\mathbf{v}_{2}\) are given by the following vector equations, obtained by combining the previous relations  
\[\left\{\begin{array}{l}\Delta\mathbf{v}_{1}=N_{f}^{-1}[\delta\mathbf{r}_{T} (\Delta t_{tot})-\Delta\mathbf{r}(t_{k})]-\delta\mathbf{v}(t_{k}^{-})\\ \Delta\mathbf{v}_{2}=\delta\mathbf{v}_{T}(\Delta t_{tot})-\left[S_{f}\delta \mathbf{r}(t_{k})+T_{f}\delta\mathbf{v}(t_{k}^{+})\right]\end{array}\right., \tag{8}\]  
where subscript \(f\) denotes the value of the respective matrix, evaluated at \(\tau=\Delta t_{tot}-t_{k}\). Let \(\Delta t_{S}\) represent the time interval between two consecutive iterations. The guidance algorithm is based on the following iterative steps, to perform at each time \(t_{k}\):  
* calculate \(t_{k+1}=t_{k}+\Delta t_{S}\); if \(t_{k+1}\geq\Delta t_{tot}\), then set \(t_{k+1}=\Delta t_{tot}\);
* evaluate the displacement vectors \(\delta\mathbf{r}\) and \(\delta\mathbf{v}_{1}\) at \(t_{k}^{-}\);
* calculate \(\Delta\mathbf{v}_{1}\) by using Equation (8);using the definitions of the displaced position and velocity coordinates, obtain the spherical coordinates of position and velocity of the chaser (\(r\), \(\xi\), \(\phi\), \(v_{r}\), \(v_{t}\), \(v_{k}\)) at \(t_{k}\);
* propagate numerically the nonlinear Equation (1) in the interval \([t_{k},t_{k+1}]\);
* if \(t_{k+1}=\Delta t_{tot}\), then evaluate \(\delta\mathbf{v_{2}}\) at \(\Delta t_{tot}\).  
It is worth remarking that the algorithm at hand evaluates iteratively \(\delta\mathbf{v_{1}}\), whereas \(\delta\mathbf{v_{2}}\) is evaluated only at the final time.