# Application and Assessment of Cooperative Localization in Three-Dimensional Vehicle Networks
## 6 Multi-Vehicle System Model
### Measurement Equations  
Consider a base vehicle \(b\) acquiring a relative pose measurement of a target vehicle \(t\). A problem with quaternions is that they are not kinematic quantities that can be directly measured by any sensor. Thus, we assume the rotation matrix representing the relative orientation of \(t\) with respect to \(b\), perhaps derived from Euler angles, is the measured quantity. We then derive the quaternions corresponding to the relative rotation matrix. Similarly, the relative range, bearing angle, and elevation angle of \(t\) with respect to \(b\) are the quantities normally measured that may be used to determine the relative position. Here, we define the measurement output vector, z, in terms of the relative position vector, \(r\), and the three components of the quaternion vector \(q\), which are determined from the relative orientation matrix:  
\[\mathbf{z}= \begin{bmatrix}r\\ q\end{bmatrix} . \tag{14}\]  
Note that the \(q_{0}\) component is not needed since it can be derived from \(q\) using the unit-length constraint equation. Using the system states, the relative position \(r\) can be written as:  
\[r=R_{0}^{b^{\top}}r_{0}^{ht}=R_{0}^{b^{\top}}(r_{0}^{\top}-r_{0}^{b})=R_{0}^{b ^{\top}}r_{0} \tag{15}\]  
Figure 2: Pose geometry for a two-vehicle network. where \(r_{0}=r_{0}^{\top}-r_{0}^{b}\). Quaternions representing the relative orientation are derived from the relative rotation matrix:  
\[R(\bar{q})={R_{0}^{b}}^{\top}(\bar{q}^{b})R_{0}^{t}(\bar{q}^{t}) \tag{16}\]  
Vector \(q\) is then derived from the elements of \(R(\bar{q})\)[46]. Next, we linearize the measurement output equations, \(\mathbf{z}=H\mathbf{x}\) where:  
\[H= \begin{bmatrix}H^{b}&H^{t}\end{bmatrix} = \begin{bmatrix}-{R_{0}^{b}}^{\top}& \frac{\partial r}{\partial\bar{q}^{b}}&{R_{0}^{b}}^{\top}&{0_{3\times 4}}\\ {0_{3\times 3}}&\frac{\partial q}{\partial\bar{q}^{b}}&{0_{3\times 3}}&\frac{ \partial q}{\partial\bar{q}^{t}}\end{bmatrix} . \tag{17}\]  
The term \(\frac{\partial r}{\partial\bar{q}^{b}}\) can be written in terms of its components as \(\frac{\partial r}{\partial\bar{q}^{b}}= \begin{bmatrix}\frac{\partial r}{ \partial q_{0}^{b}}&\frac{\partial r}{\partial\bar{q}^{b}}\end{bmatrix} \). These components can then be defined similar to (11) as:  
\[\frac{\partial r}{\partial q_{0}^{b}} =[(4q_{0}^{b})I_{3}+2S(q^{b})]r_{0} \tag{18a}\] \[\frac{\partial r}{\partial\bar{q}^{b}} =2q^{b}r_{0}^{\top}+2(q^{b^{\top}}r_{0})I_{3}-2q_{0}^{b}S(r_{0}). \tag{18b}\]  
Assuming \(q_{0}\
eq 0\), the remaining derivative terms can be shown as:  
\[\frac{\partial q}{\partial\bar{q}^{b}}=\frac{1}{4q_{0}}\frac{\partial p}{ \partial\bar{q}^{b}}\quad,\quad\frac{\partial q}{\partial\bar{q}^{t}}=\frac{1 }{4q_{0}}\frac{\partial p}{\partial\bar{q}^{t}} \tag{19}\]  
where \(p\) is represented in terms of the elements of the matrix \(R(\bar{q})\)  
\[p=[R_{32}-R_{23}\quad R_{13}-R_{31}\quad R_{21}-R_{12}]^{\top}, \tag{20}\]  
and the components of (19) are derived as:  
\[ \begin{split}\frac{\partial p}{\partial\bar{q}_{0}^{b}}& =16q_{0}^{b}q_{0}^{t}q_{0}^{t}+2[R_{0}^{t}-tr(R_{0}^{t})I_{3}]q^{b} \\ \frac{\partial p}{\partial q_{0}^{t}}&=-16q_{0}^{b}q _{0}^{t}q_{0}^{b}-2[R_{0}^{b}-tr(R_{0}^{b})I_{3}]q^{t}\\ \frac{\partial p}{\partial\bar{q}^{b}}&=2q_{0}^{b}[R_{0 }^{t}-tr(R_{0}^{t})I_{3}]-2S(q^{b}){R_{0}^{t}}^{\top}+2S({R_{0}^{t}}^{\top}q^{ b})\\ \frac{\partial p}{\partial q^{t}}&=-2q_{0}^{t}[R_{0 }^{b}-tr(R_{0}^{b})I_{3}]+2S(q^{t}){R_{0}^{b}}^{\top}-2S({R_{0}^{b}}^{\top}q^{ t})\\ \end{split}  \tag{21}\]  
If \(q_{0}=0\), components of \(\frac{\partial q}{\partial q^{b}}\) and \(\frac{\partial q}{\partial q^{t}}\) are derived as:  
\[ \begin{split}\frac{\partial q_{i}}{\partial q_{j}^{b}}& =\frac{1}{4q_{i}}\frac{\partial R_{ii}}{\partial q_{j}^{b}},\quad q _{i}\
eq 0\\ \frac{\partial q_{i}}{\partial q_{j}^{t}}& =\frac{1}{4q_{i}}\frac{\partial R_{ii}}{\partial q_{j}^{t}},\quad q _{i}\
eq 0,\quad i,j=1,2,3\\ \frac{\partial q_{i}}{\partial q_{j}^{b}}& =\frac{\partial q_{i}}{\partial q_{j}^{t}}=0,\quad q_{i}=0\\ \end{split}