Ultra-Wideband-Based Localization for Quadcopter Navigation----读后感

Ultra-Wideband-Based Localization for Quadcopter Navigation----读后感

\qquad 文章内容为使用UWB测距，建立无人机运动方程和观测方程，使用EKF算法获取无人机精确位置估计。
\qquad 使用UWB测距，观测值和实际距离值之间的关系可以使用线性方程表示：
$$r=ad+b+ϵ$$其中，$r$作为实际观测值，$d$作为距离真实值，(定参时两个数值皆已知)$ϵ$作为零均值随机误差。线性方程参数$\hat{a},\hat{b}$可以使用线性回归方法估计，随机误差方差$\sigma$可有下式获得：
$$\hat{a}=\frac{{\sum }_{i=1}^n(d_i-\bar{d})(r_i-\bar{r})}{{\sum }_{i=1}^n(d_i-\bar{d}{)}^2}$$ $$\hat{b}=\bar{r}-\hat{a}\bar{d}$$$$\hat{\sigma }=\frac{{\sum }_{i=1}^n(r_i-\hat{a}{d}_i-\hat{b}{)}^2}{n-2}$$其中，$\bar{d}、\bar{r}$皆为平均值。距离估计值写位$$\hat{d}=\tilde{a}\hat{r}+\tilde{b}$$其中，$\tilde{a}=\frac{1}{\hat{a}},\tilde{b}=-\frac{\hat{b}}{\hat{a}}$其渐进均方估计误差为:
$$R=(\hat{\sigma }/\hat{a}{)}^2$$ \qquad 基于UWB测距的无人机位置估计方法可以使用两种，方法一，使用无人机到UWB锚节点的测距值作为观测值，建立无人机运动方程，使用EKF算法获得无人机位置估计，方法二，使用三点测距法直接求解，使用牛顿法获得数值解。
\qquad 方法一，无人机运动方程为：
$${\left[\begin{array}{c}p\\ v\end{array}\right]}_{k+1}=\left[\begin{array}{cc}I& \Delta t_{k}I\\ 0& 1\end{array}\right]{\left[\begin{array}{c}p\\ v\end{array}\right]}_k+\left[\begin{array}{c}\frac{1}{2}{a}_k(\Delta t_k{)}^2\\ a_k\Delta t_k\end{array}\right]$$即状态转移(无人机运动方程)可以写为：
$$x_{k+1}=A_k{x}_k+\epsilon ,E({\epsilon }_k{\epsilon }_k^T)=Q_k$$，把加速度看作过程噪声。观测方程由UWB测距获得$$d_k=||p_k-p^a||+{\eta }_k,E({\eta }_k{\eta }_k^T)=R$$其中，$d_k$为观测值，但是非线性量，将其转化为线性为在估计值处展开$$d_k\approx {\hat{d}}_k+\frac{1}{{\hat{d}}_k}({\hat{p}}_k-p^a{)}^T(p-{\hat{p}}_k)+{\eta }_k转置有东西!!!!$$在$EKF$中，观测方程的线性化过程为：$$\begin{array}{rl}{y}_k& =h_k({\hat{x}}_k^{-},0)+\frac{\partial h_k}{\partial x}{|}_{{\hat{x}}_k^{-}}(x_k-{\hat{x}}_k^{-})+\frac{\partial h_k}{\partial v}{|}_{{\hat{x}}_k^{-}}{v}_k\\ & =h_k({\hat{x}}_k^{-},0)+H_k(x_k-{\hat{x}}_k^{-})+M_k{v}_k\\ & =H_k{x}_k+[h_k({\hat{x}}_k^{-},0)-H_k{\hat{x}}_k^{-}]+M_k{v}_k\\ & =H_k{x}_k+z_k+{\tilde{v}}_k\end{array}$$按照$KF$滤波过程$y_k=h_k(x_k,v_k)$ $$\begin{array}{rl}{\hat{x}}_k^{+}& ={\hat{x}}_k^{-}+K_k(y_k-H_k{\hat{x}}_k^{-}-z_k)\\ & ={\hat{x}}_k^{-}+K_k(y_k-h_k({\hat{x}}_k^{-},0))\end{array}$$于是，预测方程：
$$\begin{array}{rl}{\hat{x}}_k^{-}& =A_{k-1}{\hat{x}}_{k-1}^{+}\\ {\hat{P}}_k^{-}& =A_{k-1}{\hat{P}}_{k-1}^{+}{A}_{k-1}^T+Q_{k-1}\end{array}$$线性化后得到观测方程为：$$\left[\begin{array}{c}{d}_k^1\\ d_k^2\\ ⋮\\ d_k^k\end{array}\right]=\left[\begin{array}{c}{\hat{d}}_k^1\\ {\hat{d}}_k^2\\ ⋮\\ {\hat{d}}_k^k\end{array}\right]+\left[\begin{array}{cccc}\frac{1}{{\hat{d}}_k^1}({\hat{x}}_k-x^1)& \frac{1}{{\hat{d}}_k^1}({\hat{y}}_k-y^1)& 0& 0\\ \frac{1}{{\hat{d}}_k^2}({\hat{x}}_k-x^2)& \frac{1}{{\hat{d}}_k^2}({\hat{y}}_k-y^2)& 0& 0\\ ⋮& & & \\ \frac{1}{{\hat{d}}_k^k}({\hat{x}}_k-x^k)& \frac{1}{{\hat{d}}_k^k}({\hat{y}}_k-y^k)& 0& 0\end{array}\right]\ast \left[\begin{array}{c}{x}_k-{\hat{x}}_k\\ y_k-{\hat{y}}_k\\ v_{x_k}\\ v_{y_k}\end{array}\right]$$改写为$${\mathbf{d}}_k={\hat{\mathbf{d}}}_k+{\mathbf{H}}_k({\mathbf{x}}_{\mathbf{k}}-{\hat{\mathbf{x}}}_{\mathbf{k}})$$ 所以状态更新方程为：$${\hat{x}}_k^{+}={\hat{x}}_k^{-}+K_k(d_k-{\hat{d}}_k)$$参数更新为：
$$K_k={\hat{P}}_k^{-}{H}_k^T(H_k{\hat{P}}_k^{-}{H}_k^T+R{)}^{-1}$$$${\hat{P}}_k^{+}=(I-K_k{H}_k){\hat{P}}_k^{-}$$
\qquad 第二种方法为三边定位法，使用TOA或者TDOA即可测的，只要求解位置状态时，通过无人机到锚节点的距离和锚节点的位置即可建立求解方程组，求解位置状态。
\qquad 无论哪种方法，定位算法容易受到异常测量值的影响，导致算法鲁棒性的降低。对于异常值，可以使用马氏距离获得，马氏距离即计算在方差维度下测量值和估计值的差别，由此设置阈值，提取离群点，马氏距离使用测量值与估计值差的平方除以过程噪声和测量噪声的方差。$$D=\frac{(d_k-{\hat{d}}_k{)}^2}{H_k{\hat{P}}_k^{-}{H}_k^T+R}$$