# Cramer-Rao Lower Bounds on 3D Position and Orientation Estimation in Distributed Ranging Systems
## 5 Simulation Results
### Estimation Preliminaries  
For the given model, the expected log-likelihood function \(\mathcal{L}(\theta)=\mathrm{E}[\ln p_{\tau}(t;h)]\) reduces to a least-square problem. We adopt the ordinary least squares (LS) formulation from [53] and present it here for the sake of completeness. Define a matrix \(\mathbf{X}\) and elements of a column vector \(\mathbf{y}\) for \(i\in\{1,2,\cdots,N\}\) as  
\[\mathbf{X}=\begin{bmatrix}2(x_{1}-\bar{x})&2(y_{1}-\bar{y})\\ 2(x_{2}-\bar{x})&2(y_{2}-\bar{y})\\ \vdots&\vdots\\ 2(x_{N}-\bar{x})&2(y_{N}-\bar{y})\end{bmatrix}\quad;\quad\mathbf{y}[i]=d_{i}^ {2}-d_{i}^{2}+\mathbf{X}\bar{y} \tag{48}\]  
where \(g_{i}=[x_{i},y_{i}]^{T}\) is the location of \(N\) ground node antennae, \(\bar{g}=[\bar{x},\bar{y}]^{T}\) is their mean location, and \(\bar{d}_{i}=\|g_{i}-\bar{g}\|^{1/2}\) is the distance between \(g_{i}\) and \(\bar{g}\). The unbiased, linearized, least-square estimate of position \(h\) is simply  
\[\hat{h}_{\mathrm{LS}}=(\mathbf{X}^{T}\mathbf{X})^{-1}\mathbf{X}^{T}Y \tag{49}\]  
For the given model, we propose a maximum-likelihood orientation estimator. The expected log-likelihood function \(\mathcal{L}(\theta)=\mathrm{E}\Big{[}\ln p_{\overline{\mathbf{g}}}(\overline {\mathbf{p}};\overline{\mathbf{\sigma}},\theta,\kappa)\Big{]}\) is  
\[\mathcal{L}(\theta)=\sum_{k=1}^{M}\kappa_{k}\,\overline{b}_{k}^{T}\,\mathbf{ R}(\theta)\;\overline{v}_{k}+\text{constants}. \tag{50}\]  
Since this is a convex function, there exists a unique, unambiguous angle \(\theta\in[0,\pi]\) for which \(\mathcal{L}(\theta)\) is maximized, which is computed as  
\[\frac{d}{d\theta}\mathcal{L}(\theta)=\sum_{k=1}^{N}\kappa_{k}\,\overline{b}_ {k}^{T}\mathbf{S}(\theta)\overline{v}_{k}=0, \tag{51}\]  
where \(\mathbf{S}(\theta)=\frac{d}{d\theta}\mathbf{R}(\theta)\). Therefore, the maximum-likelihood estimate of orientation is obtained by solving Equation (51) as  
\[\hat{\theta}_{ML}=\arctan\Bigg{\{}\frac{\sum_{k=1}^{M}\kappa_{k}\,\overline{b }_{k}^{T}\mathbf{R}(\frac{\pi}{2})\,\overline{v}_{k}}{\sum_{k=1}^{M}\kappa_{k }\,\overline{b}_{k}^{T}\overline{v}_{k}}\Bigg{\}} \tag{52}\]  
It is well established that \(\kappa=A^{-1}(\rho)\) and, for a two-dimensional space, \(A(\cdot)=I_{1}(\cdot)/I_{0}(\cdot)\) is a ratio of modified Bessel functions of the first kind and order one and zero; there are multiple approximations to \(A^{-1}(\cdot)\)[54], and we adopt the one provided in [55].  
We attain a comprehensive measure for concentration factor \(\kappa\) by assuming \(\kappa_{k}=\kappa\) for \(k\in\{1,2,\cdots,M\}\), \(\sigma_{h_{i}}=\sigma_{p}\) for \(i\in\{1,2,\cdots,N\}\), and approximating \(\kappa\) as  
\[\hat{\kappa}=\frac{\bar{\rho}\left(2-\bar{\rho}^{2}\right)}{1-\bar{\rho}^{2}} \quad;\quad\bar{\rho}=e^{-\sigma_{p}^{2}}, \tag{53}\]  
where \(0\leq\beta\leq 1\) is the mean resultant length. When the standard deviation in position estimation \(\sigma_{p}>1\) meter, the concentration factor \(\kappa\ll 1\). In those scenarios, the [PERSON] model assumption fails, and the vectors \(\vec{b}_{k}\) do not contain any information regarding rotations \(\theta\).