# A Real-Time Simulator for Navigation in GNSS-Denied Environments of UAV Swarms
## 2 Architecture
### Inertial-Aided Scene Matching Navigation (ISMN)
#### 2.1.1 Reference Map from INS Propagation  
IMU data are used to narrow the range of satellite images that we need to analyze to perform localization. The propagator is based on the relative acceleration and velocity of the INS model, which is accurate over a short time. As shown in Figure 3, the position of the UAV at time k is \(p_{k}\). After a short time, the position of the UAV at time \(k+1\) is \(p_{k+1}\). The distance at which the UAV flies forward is \(\Delta p\), which can be obtained by INS propagation. The deviation \(\delta p\) caused by the drone attitude can be obtained through parameters such as the drone flight attitude and altitude. The specific derivation process is as follows.  
Figure 3: Reference map from INS propagation.  
Figure 2: ISMN system architecture.  
The velocity update equation with respect to the East, North, Up (ENU) frame in the simplified inertial navigation algorithm is shown in Equation (1).  
\[\dot{v}^{n}(t)=C_{b}^{n}f^{b}(t)-(2\omega_{ie}^{n}(t)+\omega_{en}^{n}(t))\times v ^{n}(t)+g^{n}(t) \tag{1}\]  
\(C_{b}^{n}\) is the transformation from the body frame (\(b\) frame) to the local geographic frame (\(n\) frame), \(v^{n}=\begin{bmatrix}v_{e}&v_{n}&v_{u}\end{bmatrix}\) represents the velocity in the n frame, \(f^{b}\) is the specific force vector as measured by accelerometers in the b frame, \(\omega_{ie}^{n}\) is the rotation rate of the Earth expressed in the n frame, \(\omega_{en}^{n}\) is the transport rate, which is only related to the longitude and latitude, and \(g^{n}\) is the local gravitational acceleration vector.  
The position update equation in longitude and latitude is shown in Equation (2).  
\[\dot{p}=M_{pv}v^{n} \tag{2}\]  
\[p=\begin{bmatrix}L\\ \lambda\\ h\end{bmatrix} \tag{3}\]  
\[M_{pv}=\begin{bmatrix}0&1/R_{Mh}&0\\ \sec L/R_{Nh}&0&0\\ 0&0&1\end{bmatrix} \tag{4}\]  
\[R_{Mh}=R_{M}+h,\ R_{Nh}=R_{N}+h \tag{5}\]  
\[R_{M}=\frac{R_{N}(1-e^{2})}{\left(1-e^{2}\sin^{2}L\right)} \tag{6}\]  
\[R_{N}=\frac{R_{e}}{\left(1-e^{2}\sin^{2}L\right)^{1/2}} \tag{7}\]  
\[e=\sqrt{2f-f^{2}} \tag{8}\]  
where \(L\) represents longitude, \(\lambda\) represents latitude, \(R_{e}\) is the Earth's radius, \(e\) is the eccentricity of an ellipsoid and \(f\approx 1/298.257\) is the oblateness of the ellipsoid shape.  
According to the current longitude, latitude, INS propagation, attitude and height of the drone, we can predict the next feature position in the base map.  
The next drone position can be obtained as follows:  
\[p_{k+1}=p_{k}+\Delta p \tag{9}\]  
Concerning the attitude of the drone, the positions of images captured by the camera are derived as  
\[p_{k+1}^{c}=p_{k+1}+\delta p \tag{10}\]  
where  
\[\delta p=f\left(\begin{bmatrix}\frac{E(0)}{E(2)}\\ \frac{E(1)}{E(2)}\\ \frac{E(2)}{E(2)}\end{bmatrix}h\right),\ E=C_{b}^{n}K,\ K=\begin{bmatrix}0 \\ 0\\ 1\end{bmatrix}\]  
\(\delta p\) is the drift from obliqueness, \(f()\) is a transform from the NED coordinate to the WGS-84 coordinate and \(h\) is the altitude of the UAV.  
According to \(p_{k+1}^{c}\), we can obtain the feature position in the base map, so the number of images that we need to analyze is significantly reduced. We use this method to increase the efficiency of the SMN.