# Methodology for CubeSat Debris Collision Avoidance Based on Its Active ADCS System
## 3 Materials and Methods
### Control Algorithm
The idea is to minimize a cost function as:  
\[C(R)=\frac{1}{2}{\sum_{j=1}^{n}s_{j}}\left\|v_{j}^{B}-R^{T}v_{j}^{H}\right\|^ {2} \tag{8}\]  
where \(v_{j}^{B}\) represents the normalized body-frame vectors that represent the \(j\) measurements, \(v_{j}^{H}\) is the corresponding inertial ones, and \(s_{j}\) is the level of confidence for each of the measurements [25]. If we now define the observation vectors as:  
\[p_{j}=\frac{s_{j}}{\sum_{j=1}^{n}s_{j}} \tag{9}\]  
We can then introduce the attitude profile matrix:  
\[L={\sum_{j=1}^{n}p_{j}v_{j}^{B}\left(v_{j}^{H}\right)}^{T} \tag{10}\]  
So that we can finally obtain:  
\[Q_{f}=\frac{1}{\sqrt{y_{j+\left\|y\right\|^{2}}}} \begin{bmatrix}y_{0}\\ y\end{bmatrix}  \tag{11}\]  
where \(y_{0}\) is a scalar and \(y\) is a vector given by:  
\[y_{0}=\det((\xi_{max}+Tr[B])I-C \tag{12}\]  
\[y=\left(\alpha_{1}I+\alpha_{2}C+C^{2}\right) \tag{13}\]  
Having the Davenport matrix as:  
\[C= \begin{bmatrix}Tr\{L\}&x^{T}\\ x&C-Tr\{L\}I\end{bmatrix}  \tag{14}\]  
where:  
\[x={\sum_{j=1}^{n}p_{j}v_{j}^{B}}{\times}v_{j}^{B} \tag{15}\]  
And:  
\[\alpha_{1}=\ \xi_{Max}^{2}-Tr[L]^{2}+Tr[adj(C)] \tag{16}\]  
\[\alpha_{2}=\ \xi_{Max}-Tr[C] \tag{17}\]  
Having \(I\) as the identity matrix, \(C\) the sum of the \(L\) matrix and its transpose, and \(\xi_{Max}\) is the greatest characteristic of the Davenport matrix. Now that the reconstruction step has been performed, the next phase will filter the uncertainties of the attitude estimated using an EKF. The EKF is an analytical method employed for the estimation of attitude in dynamic systems, leveraging both prior knowledge of the attitude and real-time sensor measurements to calculate the current attitude of the system. EKF has been chosen because of its ability to handle nonlinear systems by linearizing the differential equations that describe the system's dynamics, addressing KF limitations by introducing a linearization step. By the use of linearization of the nonlinear equations (between the system's attitude parameters and the sensor measurements) around the current estimated state, it will enable the application of standard Kalman filter techniques, providing accurate estimates even in the presence of nonlinear dynamics. The EKF will iteratively update its estimates, combining the information from both the prediction based on the system's dynamics and the correction based on the actual sensor measurements, refining the attitude estimates over time. Define the expected measurement on the predicted state \(x_{k+1}\), as:  
\[y_{k}=f_{1}(x_{k+1},\;n_{k}) \tag{18}\]  
where \(f_{1}\) is a non-linear differential function and \(n_{k}\) is the measurement noise. And, having:  
\[x_{k+1}=f_{2}(x_{k},\;m_{k}) \tag{19}\]  
where \(f_{2}\) is a non-linear differential function and \(m_{k}\) is the process noise. The state model serves to forecast the subsequent state of a dynamic system based on the current state, while the measurement model anticipates the observation vector using the projected state. In the context of the EKF, the linearization process is applied around an estimated orientation that undergoes continuous updates through sensor observations. At each iteration, the EKF undertakes the process of linearization of the differential equations. This involves approximating the nonlinear relationships between system variables by their linear counterparts, allowing the EKF to iteratively update predictions and measurements with improved accuracy as it refines its estimation of the system's state over time. The filter uses the first initial state \(x_{k-1}\), \(P_{k-1}\) as the state covariance matrix, \(z_{k}\) as the measurements given by the IMU, \(Q_{k}\) as the process noise covariance matrix, and \(R_{k}\) as the measurement noise covariance matrix. The maximum number of iterations of the process, denoted as N, is a parameter in the EKF that may be contingent on the specified error threshold or defined by the system designer.