基于证据融合的四旋翼飞行器姿态估计方法

摘要

本发明涉及一种基于证据融合的四旋翼飞行器姿态估计方法。本发明建立四旋翼飞行器姿态的状态方程与观测方程，其中的状态与观测噪声都被建模为三角形可能性分布函数。将该函数转换为噪声证据，然后将它们带入状态方程和观测方程与实际观测值合成后，即可生成三种描述飞行器状态的证据，利用证据组合规则实现这些证据的融合，通过区间映射工具实现三个证据在连续时刻的递归传播。对融合后证据所包含的信度赋值进行加权，即可得到飞行器姿态的估计值。本发明所提方法无需满足状态和观测噪声概率分布已知的苛刻要求，而只需限定噪声有界且可用简单的三角形函数描述。这增加了在实际姿态估计中的实用性，与已有经典方法相比，也具有较高的估计精度。

说明书

技术领域

本发明涉及一种基于证据融合的四旋翼飞行器姿态估计方法，属于飞行器姿 态估计与控制技术领域。

背景技术

现代传感器技术、计算机技术和各种智能信息处理技术的快速发展，推动了 面向各种功能需求的航空航天飞行器在国防和民用领域中的广泛应用。由于功能 需求多样且应用环境复杂，使得对飞行器姿态估计的稳定性和精准性方面提出了 较高的要求，因此对飞行器姿态估计方法的研究具有十分重要的理论和实际意 义。

根据飞行器的不同应用场景和性能要求，可采用不同的姿态测量系统及估计 方法，但其本质都是以非线性滤波为基础。目前广泛使用的非线性滤波方法是扩 展卡尔曼滤波(Extended Kalman Filter，EKF)和无迹卡尔曼滤波(Unscented Kalman  Filter，UKF)等，它们都要求系统的状态噪声与观测噪声的概率分布等统计特性 精确已知，但是在实际中，通常没有足够的先验数据用于统计出精确的概率分布， 更容易得到的是噪声变化的大致界限。所以，研究噪声有界下的飞行器姿态估计 问题，更加贴合实际应用环境，且具有较强的理论研究价值。

发明内容

本发明的目的在于，所提出的一种基于证据融合的四旋翼飞行器姿态估计方 法，可以在状态与观测噪声有界情况下，建立关于飞行器姿态状态方程、观测方 程及实际测量的证据，通过证据融合得到姿态估计值，利用区间映射工具实现证 据的传播，从而解决状态和观测噪声统计分布不能精确获得的情况下的姿态估计 问题。

本发明提出的一种基于证据融合的四旋翼飞行器姿态估计方法，包括以下各 步骤：

（1）建立四旋翼飞行器姿态的状态方程与观测方程，具体步骤如下

（1-1）建立四旋翼飞行器姿态的状态方程如式(1)所示：

x_k+1＝Ax_k+Q(v)     (1)

式(1)中，k=1,2,3,…,表示时刻，$x_k={\left(\begin{array}{cccc}{q}_{0,k}& q_{1,k}& q_{2,k}& q_{3,k}\end{array}\right)}^T$表示k时刻四旋翼飞行 器姿态的四元数状态向量，A为状态转移矩阵

$A=\left(\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right)$

$Q\left(v\right)={\left(\begin{array}{cccc}Q\left(v_{q0}\right)& Q\left(v_{q1}\right)& Q\left(v_{q2}\right)& Q\left(v_{q3}\right)\end{array}\right)}^T$为状态噪声函数向量，$v={\left(\begin{array}{cccc}{v}_{q0}& v_{q1}& v_{q2}& v_{q3}\end{array}\right)}^T$为 四元数状态的噪声变化向量，并有

其为一个三角形可能性分布函数，其中，（1-2）建立四旋翼飞行器姿态的观测方程如式(3)所示：

z_k+1＝h(x_k+1)+R(w)     (3)

其中，表示k+1时刻四旋翼飞行器姿态的观测向量，θ_k+1和ψ_k+1分别为四旋翼飞行器k+1时刻的横滚角、俯仰角、偏航角的取值，状 态向量x_k+1到观测向量z_k+1的转换函数为：

$h\left(x_{k+1}\right)=\left(\begin{array}{c}\arctan \left(\frac{2(q_{2,k+1}{q}_{3,k+1}+q_{0,k+1}{q}_{1,k+1})}{1-2(q_{1,k+1}^2+q_{2,k+1}^2)}\right)\\ \arctan (-2(q_{1,k+1}{q}_{3,k+1}-q_{0,k+1}{q}_{2,k+1}))\\ \arctan \left(\frac{2(q_{1,k+1}{q}_{2,k+1}+q_{0,k+1}{q}_{3,k+1})}{1-2(q_{2,k+1}^2+q_{3,k+1}^2)}\right)\end{array}\right)---\left(4\right)$

记r₁、r₂和r₃分别表示四旋翼飞行器姿态的横滚角、俯仰角、偏航角，则 $R\left(w\right)={\left(\begin{array}{ccc}R\left(w_{r_1}\right)& R\left(w_{r_2}\right)& R\left(w_{r_3}\right)\end{array}\right)}^T$为观测噪声函数向量，$w={\left(\begin{array}{ccc}{w}_{r_1}& w_{r_2}& w_{r_3}\end{array}\right)}^T$是四旋 翼飞行器姿态的观测噪声变化向量，并有

其为一个三角形可能性分布函数，其中$w_{r_l}\in [w_{a,r_l},w_{b,r_l}],w_{c,r_l}=(w_{a,r_l}+w_{b,r_l})/2;$

四旋翼飞行器姿态的四元数状态向量$x_k={\left(\begin{array}{cccc}{q}_{0,k}& q_{1,k}& q_{2,k}& q_{3,k}\end{array}\right)}^T$中的各元素与 观测向量$z_k={\left(\begin{array}{ccc}{r}_{1,k}& r_{2,k}& r_{3,k}\end{array}\right)}^T$各元素之间的转换关系函数如式(6)所示

$q_{i,k}=\frac{q_{i,k}^{\prime }}{\sqrt{q_{0,k}^{\prime 2}+q_{1,k}^{\prime 2}+q_{2,k}^{\prime 2}+q_{3,k}^{\prime 2}}}---\left(6\right)$

其中

$q_{0,k}^{\prime }=\cos \frac{r_{1,k}}{2}\cos \frac{r_{2,k}}{2}\cos \frac{r_{3,k}}{2}+\sin \frac{r_{1,k}}{2}\sin \frac{r_{2,k}}{2}\sin \frac{r_{3,k}}{2}---\left(7\right)$

$q_{1,k}^{\prime }=\sin \frac{r_{1,k}}{2}\cos \frac{r_{2,k}}{2}\cos \frac{r_{3,k}}{2}-\cos \frac{r_{1,k}}{2}\sin \frac{r_{2,k}}{2}\sin \frac{r_{3,k}}{2}---\left(8\right)$

$q_{2,k}^{\prime }=\cos \frac{r_{1,k}}{2}\sin \frac{r_{2,k}}{2}\cos \frac{r_{3,k}}{2}+\sin \frac{r_{1,k}}{2}\cos \frac{r_{2,k}}{2}\sin \frac{r_{3,k}}{2}---\left(9\right)$

$q_{3,k}^{\prime }=\cos \frac{r_{1,k}}{2}\cos \frac{r_{2,k}}{2}\sin \frac{r_{3,k}}{2}-\sin \frac{r_{1,k}}{2}\sin \frac{r_{2,k}}{2}\cos \frac{r_{3,k}}{2}---\left(10\right)$

（2）构造状态噪声函数向量Q(v)关于四元数状态向量x_k中元素q_i,k的证据 $(F_Q^{q_i},B_Q^{q_i}),i=\mathrm{0,1,2,3},$具体步骤如下：

（2-1）构造状态噪声函数向量Q(v)关于四元数状态向量x_k元素q_i,k的初始证 据其中${\bar{F}}_Q^{q_i}=\left\{\right[L_{{\alpha }_0}^{q_i-},R_{{\alpha }_0}^{q_i+}],[L_{{\alpha }_1}^{q_i-},R_{{\alpha }_1}^{q_i+}],...,[L_{{\alpha }_j}^{q_i-},R_{{\alpha }_j}^{q_i+}],...,[L_{{\alpha }_{p-1}}^{q_i-},R_{{\alpha }_{p-1}}^{q_i+}\left]\right\},$表示关于 k时刻四旋翼飞行器姿态的四元数状态向量$x_k={\left(\begin{array}{cccc}{q}_{0,k}& q_{1,k}& q_{2,k}& q_{3,k}\end{array}\right)}^T$中元素 q_i,k(i=0,1,2,3)的状态噪声区间集合，为中的第j个取值为区间的元素， j=0,1,…,p-1，p∈[3,5]，为它的左端点，为它的右端点，且有

$\left(\begin{array}{c}{L}_{{\alpha }_j}^{q_i-}=\underset{v_{q_i}}{\min }\left\{v_{q_i}\right|Q\left(v_{q_i}\right)\ge {\alpha }_j\}\\ R_{{\alpha }_j}^{q_i+}=\underset{v_{q_i}}{\max }\left\{v_{q_i}\right|Q\left(v_{q_i}\right)\ge {\alpha }_j\}\end{array}\right)---\left(11\right)$

其中α_j＝j/p，则有

$[L_{{\alpha }_0}^{q_i-},R_{{\alpha }_0}^{q_i+}]\supset [L_{{\alpha }_1}^{q_i-},R_{{\alpha }_1}^{q_i+}]\supset ...[L_{{\alpha }_j}^{q_i-},R_{{\alpha }_j}^{q_i+}]\supset ...\supset [L_{{\alpha }_{p-2}}^{q_i-},R_{{\alpha }_{p-2}}^{q_i+}]\supset [L_{{\alpha }_{p-1}}^{q_i-},R_{{\alpha }_{p-1}}^{q_i+}]$

是中各区间元素的信度组成的集合，并有

${\bar{B}}_Q^{q_i}=\left\{{\bar{B}}_Q^{q_i}\right([L_{{\alpha }_0}^{q_i-},R_{{\alpha }_0}^{q_i+}]),{\bar{B}}_Q^{q_i}([L_{{\alpha }_1}^{q_i-},R_{{\alpha }_1}^{q_i+}]),...,{\bar{B}}_Q^{q_i}([L_{{\alpha }_j}^{q_i-},R_{{\alpha }_j}^{q_i+}]),...,{\bar{B}}_Q^{q_i}([L_{{\alpha }_{p-1}}^{q_i-},R_{{\alpha }_{p-1}}^{q_i+}\left]\right)\}$

其元素的取值如式(12)所示

${\bar{B}}_Q^{q_i}\left(\right[L_{{\alpha }_j}^{q_i-},R_{{\alpha }_j}^{q_i+}\left]\right)=\left(\begin{array}{cc}{\alpha }_{j+1}& j=0\\ {\alpha }_{j+1}-{\alpha }_j& j=\mathrm{1,2},...p-2\\ 1-{\alpha }_j& j=p-1\end{array}\right)---\left(12\right)$

（2-2）对步骤（2-1）获取的初始证据分别进行折扣计算，获得状 态噪声函数向量Q(v)关于四元数状态向量x_k元素q_i,k的证据其中关于 q_i,k的状态噪声区间集合为

$F_Q^{q_i}=\left\{[L_{{\alpha }_0}^{q_i-},R_{{\alpha }_0}^{q_i+}],[L_{{\alpha }_1}^{q_i-},R_{{\alpha }_1}^{q_i+}],...,[L_{{\alpha }_j}^{q_i-},R_{{\alpha }_j}^{q_i+}],...,[L_{{\alpha }_{p-1}}^{q_i-},R_{{\alpha }_{p-1}}^{q_i+}],[{\lambda ·L}_{{\alpha }_0}^{q_i-},{\lambda ·R}_{{\alpha }_0}^{q_i+}]\right\},\lambda \in \left[\mathrm{10,100}\right]$为中各区间元素的信度组成的集合

$B_Q^{q_i}=\left\{B_Q^{q_i}\right([L_{{\alpha }_0}^{q_i-},R_{{\alpha }_0}^{q_i+}]),B_Q^{q_i}([L_{{\alpha }_1}^{q_i-},R_{{\alpha }_1}^{q_i+}]),...,B_Q^{q_i}([L_{{\alpha }_j}^{q_i-},R_{{\alpha }_j}^{q_i+}]),...,B_Q^{q_i}([L_{{\alpha }_{p-1}}^{q_i-},R_{{\alpha }_{p-1}}^{q_i+}]),B_Q^{q_i}([{\lambda ·L}_{{\alpha }_0}^{q_i-},{\lambda ·R}_{{\alpha }_0}^{q_i+}]\left)\right\}$

其中

$B_Q^{q_i}\left(\right[L_{{\alpha }_j}^{q_i-},R_{{\alpha }_j}^{q_i+}\left]\right)=(1-{ϵ}_Q){\bar{B}}_Q^{q_i}\left(\right[L_{{\alpha }_j}^{q_i-},R_{{\alpha }_j}^{q_i+}\left]\right),j=\mathrm{0,1}...,p-1,i=\mathrm{0,1,2,3}---\left(13\right)$

$B_Q^{q_i}\left(\right[{\lambda ·L}_{{\alpha }_0}^{q_i-},{\lambda ·R}_{{\alpha }_0}^{q_i+}])={ϵ}_Q---\left(14\right)$

这里折扣率ε_Q∈[0.03,0.08]；

（3）通过状态转移矩阵A获取k时刻关于向量x_k的元素q_i,k的预测证据 其中下标k+1|k表示利用k时刻的状态对k+1时刻的状态进 行预测，具体步骤如下：

（3-1）构建x_k元素q_i,k的状态估计证据具体步骤如下：

（a）当k=0时，取z_k的元素r_1,k r_2,k r_3,k分别带入式(7)-(10)得到

$q_{0,0|0}^{\prime }=\cos \frac{r_{1,0}}{2}\cos \frac{r_{2,0}}{2}\cos \frac{r_{3,0}}{2}+\sin \frac{r_{1,0}}{2}\sin \frac{r_{2,0}}{2}\sin \frac{r_{3,0}}{2}$

$q_{1,0|0}^{\prime }=\sin \frac{r_{1,0}}{2}\cos \frac{r_{2,0}}{2}\cos \frac{r_{3,0}}{2}-\cos \frac{r_{1,0}}{2}\sin \frac{r_{2,0}}{2}\sin \frac{r_{3,0}}{2}$

$q_{2,0|0}^{\prime }=\cos \frac{r_{1,0}}{2}\sin \frac{r_{2,0}}{2}\cos \frac{r_{3,0}}{2}+\sin \frac{r_{1,0}}{2}\cos \frac{r_{2,0}}{2}\sin \frac{r_{3,0}}{2}$

$q_{3,0|0}^{\prime }=\cos \frac{r_{1,0}}{2}\cos \frac{r_{2,0}}{2}\sin \frac{r_{3,0}}{2}-\sin \frac{r_{1,0}}{2}\sin \frac{r_{2,0}}{2}\cos \frac{r_{3,0}}{2}$

再利用式(6)得到

${\hat{q}}_{i,0|0}=\frac{q_{i,0|0}^{\prime }}{\sqrt{q_{\mathrm{0,0}|0}^{\prime 2}+q_{\mathrm{1,0}|0}^{\prime 2}+q_{\mathrm{2,0}|0}^{\prime 2}+q_{\mathrm{3,0}|0}^{\prime 2}}},i=\mathrm{0,1,2,3}$

由此得到状态估计向量${\hat{x}}_{0|0}={\left(\begin{array}{cccc}{\hat{q}}_{\mathrm{0,0}|0}& {\hat{q}}_{\mathrm{1,0}|0}& {\hat{q}}_{\mathrm{2,0}|0}& {\hat{q}}_{\mathrm{3,0}|0}\end{array}\right)}^T,$则状态估计证据中 的

$\left(\begin{array}{c}{F}_{0|0}^{q_i}=\left\{\right[L_{{\alpha }_0}^{q_i-}+{\hat{q}}_{i,0|0},R_{{\alpha }_0}^{q_i+}+{\hat{q}}_{i,0|0}],[L_{{\alpha }_1}^{q_i-}+{\hat{q}}_{i,0|0},R_{{\alpha }_1}^{q_i+}+{\hat{q}}_{i,0|0}],...,[L_{{\alpha }_j}^{q_i-}+{\hat{q}}_{i,0|0},R_{{\alpha }_j}^{q_i+}+{\hat{q}}_{i,0|0}],\\ ...,[L_{{\alpha }_{p-1}}^{q_i-}+{\hat{q}}_{i,0|0},R_{{\alpha }_{p-1}}^{q_i+}+{\hat{q}}_{i,0|0}],[\lambda ·L_{{\alpha }_0}^{q_i-}+{\hat{q}}_{i,0|0},\lambda ·R_{{\alpha }_0}^{q_i+}+{\hat{q}}_{i,0|0}]\}\end{array}\right)---\left(15\right)$

$B_{0|0}^{q_i}=B_Q^{q_i}---\left(16\right)$

亦即

$B_{0|0}^{q_i}\left(\right[L_{{\alpha }_j}^{q_i-}+{\hat{q}}_{i,0|0},R_{{\alpha }_j}^{q_i+}+{\hat{q}}_{i,0|0}\left]\right)=B_Q^{q_i}\left(\right[L_{{\alpha }_j}^{q_i-},R_{{\alpha }_j}^{q_i+}\left]\right),j=\mathrm{0,1}...,p-1,i=\mathrm{0,1,2,3}$

$B_{0|0}^{q_i}[\lambda ·L_{{\alpha }_0}^{q_i-}+{\hat{q}}_{i,0|0},\lambda ·R_{{\alpha }_0}^{q_i+}+{\hat{q}}_{i,0|0}]=B_Q^{q_i}\left(\right[{\lambda ·L}_{{\alpha }_0}^{q_i-},{\lambda ·R}_{{\alpha }_0}^{q_i+}\left]\right)$

（b）k≥1时，由递归计算得到状态估计向量则 元素q_i,k的状态估计证据为

$\left(\begin{array}{c}{F}_{k|k}^{q_i}=\left\{\right[L_{{\alpha }_0}^{q_i-}+{\hat{q}}_{i,k|k},R_{{\alpha }_0}^{q_i+}+{\hat{q}}_{i,k|k}],[L_{{\alpha }_1}^{q_i-}+{\hat{q}}_{i,k|k},R_{{\alpha }_1}^{q_i+}+{\hat{q}}_{i,k|k}],...,[L_{{\alpha }_j}^{q_i-}+{\hat{q}}_{i,k|k},R_{{\alpha }_j}^{q_i+}+{\hat{q}}_{i,k|k}],\\ ...,[L_{{\alpha }_{p-1}}^{q_i-}+{\hat{q}}_{i,k|k},R_{{\alpha }_{p-1}}^{q_i+}+{\hat{q}}_{i,k|k}],[\lambda ·L_{{\alpha }_0}^{q_i-}+{\hat{q}}_{i,k|k},\lambda ·R_{{\alpha }_0}^{q_i+}+{\hat{q}}_{i,k|k}]\}\end{array}\right)---\left(17\right)$

$B_{k|k}^{q_i}=B_Q^{q_i}---\left(18\right)$

亦即

$B_{k|k}^{q_i}\left(\right[L_{{\alpha }_j}^{q_i-}+{\hat{q}}_{i,k|k},R_{{\alpha }_j}^{q_i+}+{\hat{q}}_{i,k|k}\left]\right)=B_Q^{q_i}\left(\right[L_{{\alpha }_j}^{q_i-},R_{{\alpha }_j}^{q_i+}\left]\right),j=\mathrm{0,1}...,p-1,i=\mathrm{0,1,2,3}$$B_{k|k}^{q_i}[\lambda ·L_{{\alpha }_0}^{q_i-}+{\hat{q}}_{i,k|k},\lambda ·R_{{\alpha }_0}^{q_i+}+{\hat{q}}_{i,k|k}]=B_Q^{q_i}\left(\right[{\lambda ·L}_{{\alpha }_0}^{q_i-},{\lambda ·R}_{{\alpha }_0}^{q_i+}\left]\right)$

（3-2）通过状态转移矩阵A获取向量x_k的元素q_i,k的预测证据 $B_{k+1|k}^{q_i}=(F_{k+1|k}^{q_i},B_{k+1|k}^{q_i}),$其中

$\left(\begin{array}{c}{F}_{k+1|k}^{q_i}=\left\{\right[A_{i+1,i+1}(L_{{\alpha }_0}^{q_i-}+{\hat{q}}_{i,k|k}),A_{i+1,i+1}(R_{{\alpha }_0}^{q_i+}+{\hat{q}}_{i,k|k})],[A_{i+1,i+1}(L_{{\alpha }_1}^{q_i-}+{\hat{q}}_{i,k|k}),A_{i+1,i+1}(R_{{\alpha }_1}^{q_i+}+{\hat{q}}_{i,k|k})],...,\\ [A_{i+1,i+1}(L_{{\alpha }_j}^{q_i-}+{\hat{q}}_{i,k|k}),A_{i+1,i+1}(R_{{\alpha }_j}^{q_i+}+{\hat{q}}_{i,k|k})],...,[A_{i+1,i+1}(L_{{\alpha }_{p-1}}^{q_i-}+{\hat{q}}_{i,k|k}),\\ A_{i+1,i+1}(R_{{\alpha }_{p-1}}^{q_i+}+{\hat{q}}_{i,k|k})],[A_{i+1,i+1}(\lambda ·L_{{\alpha }_0}^{q_i-}+{\hat{q}}_{i,k|k}),A_{i+1,i+1}(\lambda ·R_{{\alpha }_0}^{q_i+}+{\hat{q}}_{i,k|k})\left]\right\}\end{array}\right)---\left(19\right)$

这里，A_i+1,i+1表示状态转移矩阵A中，第i+1行i+1列的元素，其中i＝0,1,2,3， 并有A_i+1,i+1=1，故并有即：

$B_{k+1|k}^{q_i}\left(\right[A_{i+1,i+1}(L_{{\alpha }_j}^{q_i-}+{\hat{q}}_{i,k|k}),A_{i+1,i+1}(R_{{\alpha }_j}^{q_i+}+{\hat{q}}_{i,k|k})\left]\right)=B_{k|k}^{q_i}\left(\right[L_{{\alpha }_j}^{q_i-}+{\hat{q}}_{i,k|k},{\lambda ·R}_{{\alpha }_j}^{q_i+}+{\hat{q}}_{i,k|k}\left]\right)---\left(20\right)$

$B_{k|k}^{q_i}\left(\right[A_{i+1,i+1}(\lambda ·L_{{\alpha }_0}^{q_i-}+{\hat{q}}_{i,k|k}),A_{i+1,i+1}(\lambda ·R_{{\alpha }_0}^{q_i+}+{\hat{q}}_{i,k|k})\left]\right)=B_{k|k}^{q_i}\left(\right[{\lambda ·L}_{{\alpha }_0}^{q_i-}+{\hat{q}}_{i,k|k},{\lambda ·R}_{{\alpha }_0}^{q_i+}+{\hat{q}}_{i,k|k}\left]\right)---\left(21\right)$

（4）通过观测方程获取k+1时刻关于四旋翼飞行器姿态的观测向量z_k+1的元 素r_l,k+1的观测预测证据具体步骤如下：

（4-1）通过步骤（3）得到状态预测证据之后，分别抽取 中的一个元素进行排列组合，共计产生(p+1)⁴个四元区间组，将 这些四元区间组作为式(4)的输入量，利用MATLAB2010a软件中的工具箱intlab， 分别得到关于r_l,k+1的(p+1)⁴个区间，它们组成的区间集合为

${\bar{F}}_{k+1|k}^{r_l}=\left\{\right[{\bar{L}}_1^{r_{l,k+1k}},{\bar{R}}_1^{r_{l,k+1k}}],[{\bar{L}}_2^{r_{l,k+1k}},{\bar{R}}_2^{r_{l,k+1k}}],...,[{\bar{L}}_{\tau }^{r_{l,k+1k}},{\bar{R}}_{\tau }^{r_{l,k+1k}}],...,[{\bar{L}}_{{(p+1)}^4}^{r_{l,k+1k}},{\bar{R}}_{{(p+1)}^4}^{r_{l,k+1k}}\left]\right\},\tau =\mathrm{1,2,3},...,{(p+1)}^4---\left(22\right)$

并得到式(22)中每个区间的信度赋值组成的信度集合为

${\bar{B}}_{k+1|k}^{r_l}=\left\{{\bar{B}}_{k+1|k}^{r_l}\right([{\bar{L}}_1^{r_{l,k}},{\bar{R}}_1^{r_{l,k}}]),{\bar{B}}_{k+1|k}^{r_l}([{\bar{L}}_2^{r_{l,k+1k}},{\bar{R}}_2^{r_{l,k+1k}}]),...,{\bar{B}}_{k+1|k}^{r_l}([{\bar{L}}_{\tau }^{r_{l,k+1k}},{\bar{R}}_{\tau }^{r_{l,k+1k}}],...,{\bar{B}}_{k+1|k}^{r_l}\left(\right[{\bar{L}}_{{(p+1)}^4}^{r_{l,k+1k}},{\bar{R}}_{{(p+1)}^4}^{r_{l,k+1k}}\left]\right)\}---\left(23\right)$

其中的为所对应式(4)输入的四元区间组中每个区 间信度赋值的乘积，由式(22)和(23)可以构成关于r_l,k+1的初始观测预测证据 ${\bar{E}}_{k+1|k}^{r_l}={(\bar{F}}_{k+1|k}^{r_l},{\bar{B}}_{k+1|k}^{r_l});$

（4-2）将步骤（4-1）所得的进行简化后得到元素r_l,k+1的 观测预测证据$E_{k+1|k}^{r_l}=(F_{k+1|k}^{r_l},B_{k+1|k}^{r_l}),$其中

$F_{k+1|k}^{r_l}=\left\{\right[L_1^{r_{l,k+1k}},R_1^{r_{l,k+1k}}],[L_2^{r_{l,k+1k}},R_2^{r_{l,k+1k}}]---\left(24\right)$

$B_{k+1|k}^{r_l}=\{B_{k+1|k}^{r_l}\left(\right[L_1^{r_{l,k}},R_1^{r_{l,k}}\left]\right),B_{k+1|k}^{r_l}\left(\right[L_2^{r_{l,k}},R_2^{r_{l,k}}\left]\right)\}---\left(25\right)$

式(24)中的为中信度赋值最大的那个区间，则式(25)中该区间的 信度赋值若有多个区间信度赋值相等且最 大，则将它们取并后构成将它们的信度相加后构成 式(24)中的为剩余各区间取并后得到的区间，且 这些区间的信度相加后构成

（5）利用Dempster组合规则求出k+1时刻观测域的融合证据 ${\hat{E}}_{k+1}^{r_l}=({\hat{F}}_{k+1}^{r_l},{\hat{B}}_{k+1}^{r_l}),$具体步骤如下：

（5-1）构建z_k+1元素r_l,k+1的实时观测证据具体步骤如下

（a）构造观测噪声函数向量R(w)关于观测向量z_k+1元素r_l,k+1的初始证据 其中${\bar{F}}_R^{r_l}=\left\{\right[L_{{\alpha }_0}^{r_l-},R_{{\alpha }_0}^{r_l+}],[L_{{\alpha }_1}^{r_l-},R_{{\alpha }_1}^{r_l+}],...,[L_{{\alpha }_j}^{r_l-},R_{{\alpha }_j}^{r_l+}],...,[L_{{\alpha }_{p-1}}^{r_l-},R_{{\alpha }_{p-1}}^{r_l+}\left]\right\},$表示关于k 时刻四旋翼飞行器姿态观测向量z_k中元素r_l,k的观测噪声区间集合，为 中的第j个取值为区间的元素，j=0,1,…,p-1，p∈[3,5]，为它的左端点，为它的右端点，且有

$\left(\begin{array}{c}{L}_{{\alpha }_j}^{r_l-}=\underset{w_{q_i}}{\min }\left\{w_{r_l}\right|R\left(w_{r_l}\right)\ge {\alpha }_j\}\\ R_{{\alpha }_j}^{r_l-}=\underset{w_{r_l}}{\max }\left\{w_{r_l}\right|R\left(w_{r_l}\right)\ge {\alpha }_j\}\end{array}\right)---\left(26\right)$

其中α_j＝j/p，则有

$[L_{{\alpha }_0}^{r_l-},R_{{\alpha }_0}^{r_l+}]\supset [L_{{\alpha }_1}^{r_l-},R_{{\alpha }_1}^{r_l+}]\supset ...[L_{{\alpha }_j}^{r_l-},R_{{\alpha }_j}^{r_l+}]\supset ...\supset [L_{{\alpha }_{p-2}}^{r_l-},R_{{\alpha }_{p-2}}^{r_l+}]\supset [L_{{\alpha }_{p-1}}^{r_l-},R_{{\alpha }_{p-1}}^{r_l+}]$是中各区间元素的信度组成的集合，并有

${\bar{B}}_R^{r_l}=\left\{{\bar{B}}_R^{r_l}\right([L_{{\alpha }_0}^{r_l-},R_{{\alpha }_0}^{r_l+}]),{\bar{B}}_R^{r_l}([L_{{\alpha }_1}^{r_l-},R_{{\alpha }_1}^{r_l+}]),...,{\bar{B}}_R^{r_l}([L_{{\alpha }_j}^{r_l-},R_{{\alpha }_j}^{r_l+}]),...,{\bar{B}}_R^{r_l}([L_{{\alpha }_{p-1}}^{r_l-},R_{{\alpha }_{p-1}}^{r_l+}\left]\right)\}$其元素的取值如式(27)所示

${\bar{B}}_R^{r_l}\left(\right[L_{{\alpha }_j}^{r_l-},R_{{\alpha }_j}^{r_l+}\left]\right)=\left(\begin{array}{cc}{\alpha }_{j+1}& j=0\\ {\alpha }_{j+1}-{\alpha }_j& j=\mathrm{1,2},...p-2\\ 1-{\alpha }_j& j=p-1\end{array}\right)---\left(27\right)$

（b）对步骤（a）获取的初始证据分别进行折扣计算，获得观测噪 声向量函数R(w)关于观测向量z_k+1元素r_l,k+1的证据其中关于r_l,k+1的观测 噪声区间集合为

$F_R^{r_l}=\left\{[L_{{\alpha }_0}^{r_l-},R_{{\alpha }_0}^{r_l+}],[L_{{\alpha }_1}^{r_l-},R_{{\alpha }_1}^{r_l+}],...,[L_{{\alpha }_j}^{r_l-},R_{{\alpha }_j}^{r_l+}],...,[L_{{\alpha }_{p-1}}^{r_l-},R_{{\alpha }_{p-1}}^{r_l+}],[{\lambda ·L}_{{\alpha }_0}^{r_l-},{\lambda ·R}_{{\alpha }_0}^{r_l+}]\right\},\lambda \in \left[\mathrm{10,100}\right]$为中各区间元素的信度赋值组成的集合

$B_R^{r_l}=\left\{B_R^{r_l}\right([L_{{\alpha }_0}^{r_l-},R_{{\alpha }_0}^{r_l+}]),B_R^{r_l}([L_{{\alpha }_1}^{r_l-},R_{{\alpha }_1}^{r_l+}]),...,B_R^{r_l}([L_{{\alpha }_j}^{r_l-},R_{{\alpha }_j}^{r_l+}]),...,B_R^{r_l}([L_{{\alpha }_{p-1}}^{r_l-},R_{{\alpha }_{p-1}}^{r_l+}]),B_R^{r_l}([{\lambda ·L}_{{\alpha }_0}^{r_r-},{\lambda ·R}_{{\alpha }_0}^{r_l+}]\left)\right\}$

其中

$B_R^{r_l}\left(\right[L_{{\alpha }_j}^{r_l-},R_{{\alpha }_j}^{r_l+}\left]\right)=(1-{ϵ}_R){\bar{B}}_R^{r_l}\left(\right[L_{{\alpha }_j}^{r_l-},R_{{\alpha }_j}^{r_l+}\left]\right),j=\mathrm{0,1}...,p-1,i=\mathrm{0,1,2,3}---\left(28\right)$

$B_R^{r_l}\left(\right[{\lambda ·L}_{{\alpha }_0}^{r_l-},{\lambda ·R}_{{\alpha }_0}^{r_l+}])={ϵ}_R---\left(29\right)$

这里折扣率ε_R∈[0.03,0.08]；

（c）当从陀螺仪观测到向量$z_{k+1}={\left(\begin{array}{ccc}{r}_{1,k+1}& r_{2,k+1}& r_{3,k+1}\end{array}\right)}^T,$则构建元素r_l,k+1的观 测证据$(F_{k+1}^{r_l},B_{k+1}^{r_l})$为

$\left(\begin{array}{c}{F}_{k+1}^{r_l}=\left\{\right[L_{{\alpha }_0}^{r_l-}+r_{l,k+1},R_{{\alpha }_0}^{r_l+}+r_{l,k+1}],[L_{{\alpha }_1}^{r_l-}+r_{l,k+1},R_{{\alpha }_1}^{r_l+}+r_{l,k+1}],...,[L_{{\alpha }_j}^{r_l-}+r_{l,k+1},R_{{\alpha }_j}^{r_l+}+r_{l,k+1}],\\ ...,[L_{{\alpha }_{p-1}}^{r_l-}+r_{l,k+1},R_{{\alpha }_{p-1}}^{r_l+}+r_{l,k+1}],[\lambda ·L_{{\alpha }_0}^{r_l-}+r_{l,k+1},\lambda ·R_{{\alpha }_0}^{r_l+}+r_{l,k+1}]\}\end{array}\right)---\left(30\right)$

$B_{k+1}^{r_l}=B_R^{r_l}---\left(31\right)$

亦即：

$B_{k+1}^{r_l}\left(\right[L_{{\alpha }_j}^{r_l-}+r_{l,k+1},R_{{\alpha }_j}^{r_l+}+r_{l,k+1}\left]\right)=B_R^{r_l}\left(\right[L_{{\alpha }_j}^{r_l-},R_{{\alpha }_j}^{r_l+}\left]\right),j=\mathrm{0,1}...,p-1,l=\mathrm{1,2,3}$

$B_{k+1}^{r_l}\left(\right[{\lambda ·L}_{{\alpha }_0}^{r_l-}+r_{l,k+1},{\lambda ·R}_{{\alpha }_0}^{r_l+}+r_{l,k+1}\left]\right)=B_R^{r_l}\left(\right[\lambda ·L_{{\alpha }_0}^{r_l-},{\lambda ·R}_{{\alpha }_0}^{r_l+}\left]\right)$

（5-2）将得到的观测证据与观测预测证据利用 Dempster组合规则进行证据融合，得到观测域的融合证据

令$[L_1^{r_{l,k+1k}},R_1^{r_{l,k+1k}}]=A_1,[L_2^{r_{l,k+1k}},R_2^{r_{l,k+1k}}]=A_2,$其信度赋值为$B_{k+1|k}^{r_l}\left(A_g\right),g=\mathrm{1,2},$令 $[L_{{\alpha }_0}^{r_l-}+{\hat{r}}_{l,k+1},R_{{\alpha }_0}^{r_l+}+{\hat{r}}_{l,k+1}]=C_1,[L_{{\alpha }_1}^{r_l-}+{\hat{r}}_{l,k+1},R_{{\alpha }_1}^{r_l+}+{\hat{r}}_{l,k+1}]=C_2,...,$$[L_{{\alpha }_j}^{r_l-}+{\hat{r}}_{l,k+1},R_{{\alpha }_j}^{r_l+}+{\hat{r}}_{l,k+1}]=C_{j+1},...,[L_{{\alpha }_{p-1}}^{r_l-}+{\hat{r}}_{l,k+1},R_{{\alpha }_{p-1}}^{r_l+}+{\hat{r}}_{l,k+1}]=C_p,$$[{\lambda ·L}_{{\alpha }_0}^{r_l-}+{\hat{r}}_{l,k+1},{\lambda ·R}_{{\alpha }_0}^{r_l+}+{\hat{r}}_{l,k+1}]=C_{p+1},$其信度赋值为$B_{k+1}^{r_l}\left(C_h\right),h=\mathrm{1,2},...,p+1;$

利用Dempster组合规则将和组合后得到

其中ζ＝1,2,…,n，n≤2·(p+1)表示通过A_g∩C_h共计生成了n个不同的区间 $\left\{[L_1^{r_{l,k+1}},R_1^{r_{l,k+1}}],[L_2^{r_{l,k+1}},R_2^{r_{l,k+1}}],...,[L_{\zeta }^{r_{l,k+1}},R_{\zeta }^{r_{l,k+1}}],...,[L_n^{r_{l,k+1}},R_n^{r_{l,k+1}}]\right\},$则观测域的融合证据 ${\hat{E}}_{k+1}^{r_l}=({\hat{F}}_{k+1}^{r_l},{\hat{B}}_{k+1}^{r_l})$为

${\hat{F}}_{k+1}^{r_l}=\left\{[L_1^{r_{l,k+1}},R_1^{r_{l,k+1}}],[L_2^{r_{l,k+1}},R_2^{r_{l,k+1}}],...,[L_{\zeta }^{r_{l,k+1}},R_{\zeta }^{r_{l,k+1}}],...,[L_n^{r_{l,k+1}},R_n^{r_{l,k+1}}]\right\}$中对n个不同区间的信度赋值，可以由式(32)得到；

（6）通过观测方程的逆运算获得k+1时刻关于向量x_k+1的元素q_i,k+1的状态 域的新证据具体步骤如下：

（6-1）由步骤（5）得到的观测域的融合证据之后，分别抽 取中的一个元素进行排列组合，共计产生n³个三元区间组，将这些 三元区间组分别作为式(7-10)的输入量，利用MATLAB2010a软件中的工具箱 intlab，得到关于q_i,k+1的n³个区间，它们组成的区间集合为

${\bar{F}}_{k+1}^{q_i}=\left\{\right[{\bar{L}}_1^{q_{i,k+1}},{\bar{R}}_1^{q_{i,k+1}}],[{\bar{L}}_2^{q_{i,k+1}},{\bar{R}}_2^{q_{i,k+1}}],...,[{\bar{L}}_{\zeta }^{q_{i,k+1}},{\bar{R}}_{\zeta }^{q_{i,k+1}}],...,[{\bar{L}}_{n^3}^{q_{i,k+1}},{\bar{R}}_{n^3}^{q_{i,k+1}}\left]\right\},\zeta =\mathrm{1,2,3},...,n^3---\left(33\right)$

并可得到式(33)中每个区间的信度赋值组成的信度集合为

${\bar{B}}_{k+1}^{q_i}=\left\{{\bar{B}}_{k+1}^{q_i}\right([{\bar{L}}_1^{q_{i,k+1}},{\bar{R}}_1^{q_{i,k+1}}]),{\bar{B}}_{k+1}^{q_i}([{\bar{L}}_2^{q_{i,k+1}},{\bar{R}}_2^{q_{i,k+1}}]),...,{\bar{B}}_{k+1}^{q_i}([{\bar{L}}_{\zeta }^{q_{i,k+1}},{\bar{R}}_{\zeta }^{q_{i,k+1}}]),...,{\bar{B}}_{k+1}^{q_i}([{\bar{L}}_{n^3}^{q_{i,k+1}},{\bar{R}}_{n^3}^{q_{i,k+1}}]\left)\right\}---\left(34\right)$

其中的为所对应式(7-10)输入的三元区间组中每个区 间信度赋值的乘积，由式(33)和(34)可以构成关于q_i,k+1的状态域的初始新证据 ${\bar{E}}_{k+1}^{q_i}=({\bar{F}}_{k+1}^{q_i},{\bar{B}}_{k+1}^{q_i});$

（6-2）将步骤（6-1）所得的进行简化后得到关于元素q_i,k+1的状态域的初始新证据其中

${\hat{F}}_{k+1}^{q_i}=\{[L_1^{q_{i,k+1}},R_1^{q_{i,k+1}}],[L_2^{q_{i,k+1}},R_2^{q_{i,k+1}}]\}---\left(35\right)$

${\hat{B}}_{k+1}^{q_i}=\{{\hat{B}}_{k+1}^{q_i}[L_1^{q_{i,k+1}},R_1^{q_{i,k+1}}],{\hat{B}}_{k+1}^{q_i}[L_2^{q_{i,k+1}},R_2^{q_{i,k+1}}]\}---\left(36\right)$

式(35)中的为中信度赋值最大的那个区间，则式(36)中该区间的信 度赋值若有多个区间信度赋值相等且最大， 则将它们取并后构成将它们的信度相加后构成式 (35)中的为剩余各区间取并后得到的区间，且这些区间的信度相加后 构成${\hat{B}}_{k+1}^{q_i}\left(\right[L_2^{q_{i,k+1}},R_2^{q_{i,k+1}}]);$

（7）利用Dempster组合规则求出k+1时刻状态估计的融合证据

将步骤（6）中得到的关于向量x_k+1的元素q_i,k+1的状态域的新证据和步骤（3）中得到的关于向量x_k的元素q_i,k的预测证据利用 Dempster组合规则进行证据融合，得到k+1时刻状态估计的融合证据 $({\hat{F}}_{k+1|k+1}^{q_i},{\hat{B}}_{k+1|k+1}^{q_i});$

令$[L_1^{q_{i,k+1}},R_1^{q_{i,k+1}}]=G_1,[L_2^{q_{i,k+1}},R_2^{q_{i,k+1}}]=G_2,$其信度赋值为${\hat{B}}_{k+1}^{q_i}\left(G_g\right),g=\mathrm{1,2},$令 $[L_{{\alpha }_0}^{q_i-}+{\hat{q}}_{i,k|k},R_{{\alpha }_0}^{q_i+}+{\hat{q}}_{i,k|k}]=H_1,[L_{{\alpha }_1}^{q_i-}+{\hat{q}}_{i,k|k},R_{{\alpha }_1}^{q_i+}+{\hat{q}}_{i,k|k}]=H_2,...,$$[L_{{\alpha }_j}^{q_i-}+{\hat{q}}_{i,k|k},R_{{\alpha }_j}^{q_i+}+{\hat{q}}_{i,k|k}]=H_{j+1},...,[L_{{\alpha }_{p-1}}^{q_i-}+{\hat{q}}_{i,k|k},R_{{\alpha }_{p-1}}^{q_i+}+{\hat{q}}_{i,k|k}]=H_p,$$[{\lambda ·L}_{{\alpha }_0}^{q_i-}+{\hat{q}}_{i,k|k},{\lambda ·R}_{{\alpha }_0}^{q_i+}+{\hat{q}}_{i,k|k}]=H_{p+1},$其信度赋值为$B_{k+1|k}^{q_i}\left(H_h\right),h=\mathrm{1,2},...,p+1;$

利用Dempster组合规则将和组合后得到

其中ξ＝1,2,…，m，m≤2·(p+1)表示通过G_g∩H_h共计生成m个不同的区间，则 状态估计的融合证据为

${\hat{F}}_{k+1|k+1}^{q_i}=\left\{[L_1^{q_{i,k+1}},R_1^{q_{i,k+1}}],[L_2^{q_{i,k+1}},R_2^{q_{i,k+1}}],...,[L_{\zeta }^{q_{i,k+1}},R_{\zeta }^{q_{i,k+1}}],...,[L_m^{q_{i,k+1}},R_m^{q_{i,k+1}}]\right\}$中对m个不同区间的信度赋值，可以由式(37)得到；

（8）获得k+1时刻状态估计向量的元素的状态估计值：

${\hat{q}}_{i,k+1|k+1}=\frac{{\hat{q}}_{i,k+1|k+1}^{\prime }}{\sqrt{{\hat{q}}_{0,k+1|k+1}^{\prime 2}+{\hat{q}}_{1,k+1|k+1}^{\prime 2}+{\hat{q}}_{2,k+1|k+1}^{\prime 2}+{\hat{q}}_{3,k+1|k+1}^{\prime 2}}}---\left(38\right)$

其中，

${\hat{q}}_{i,k+1|k+1}^{\prime }=\underset{\xi =1}{\overset{m}{\Sigma }}{\hat{B}}_{k+1|k+1}^{q_i}·\frac{1}{2}(L_{\xi }^{q_{i,k+1}}+R_{\xi }^{q_{i,k+1}})---\left(39\right)$

然后将获得的k+1时刻状态估计向量 ${\hat{x}}_{k+1|k+1}={[{\hat{q}}_{0,k+1|k+1},{\hat{q}}_{1,k+1|k+1},{\hat{q}}_{2,k+1|k+1},{\hat{q}}_{3,k+1|k+1}]}^T$带入步骤（3）进行下一时刻算法迭代， 便可递推得出每一时刻的状态估计。

上述方法的关键技术在于：利用三角形可能性分布函数建模状态和观测方程 中的有界噪声，利用取值为区间的元素及其信度赋值构成状态和观测噪声的证 据。将它们带入状态方程和观测方程与实际观测值合成后，即可生成三种描述飞 行器姿态的证据，利用证据组合规则实现这些证据的融合，通过区间映射工具实 现三个证据在连续时刻的递归传播。对融合后证据所包含的信度赋值进行加权求 和，即可得到飞行器姿态的估计值。

本发明所提方法不需满足状态和观测噪声概率分布已知的苛刻要求，而只限 定噪声有界且可用简单的三角形函数描述。这增加了方法在实际姿态估计中的实 用性，且与已有经典方法相比，也具有较高的估计精度。根据本发明方法编制的 程序（编译环境LabVIEW，C++等）可以在四旋翼飞行器控制单元的处理器上运 行，得到的姿态估计结果可以用于飞行器控制，提高控制的稳定性和机动性。

附图说明

图1是本发明方法的流程框图。

图2是本发明方法实施例中四旋翼飞行器姿态的各角度估计值曲线图。

图3是本发明方法实施例中四旋翼飞行器姿态的各角度估计值绝对误差曲 线图。

具体实施方式

本发明提出的基于证据融合的四旋翼飞行器姿态估计方法，其流程图如图1 所示，包括以下各步骤：

（1）建立四旋翼飞行器姿态的状态方程与观测方程，具体步骤如下

（1-1）建立四旋翼飞行器姿态的状态方程如式(1)所示：

x_k+1＝Ax_k+Q(v)     (1)

式(1)中，k=0,1,2,3,…,表示时刻，$x_k={\left(\begin{array}{cccc}{q}_{0,k}& q_{1,k}& q_{2,k}& q_{3,k}\end{array}\right)}^T$表示k时刻四旋翼飞 行器姿态的四元数状态向量，A为状态转移矩阵

$A=\left(\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right)$

$Q\left(v\right)={\left(\begin{array}{cccc}Q\left(v_{q0}\right)& Q\left(v_{q1}\right)& Q\left(v_{q2}\right)& Q\left(v_{q3}\right)\end{array}\right)}^T$为状态噪声函数向量，$v={\left(\begin{array}{cccc}{v}_{q0}& v_{q1}& v_{q2}& v_{q3}\end{array}\right)}^T$为 四元数状态的噪声变化向量，并有

其为一个三角形可能性分布函数，其中，

（1-2）建立四旋翼飞行器姿态的观测方程如式(3)所示：

z_k+1＝h(x_k+1)+R(w)     (3)

其中，表示k+1时刻四旋翼飞行器姿态的观测向量，θ_k+1和ψ_k+1分别为四旋翼飞行器k+1时刻姿态的横滚角、俯仰角、偏航角的取值， 状态向量x_k+1到观测向量z_k+1的转换函数为：

$h\left(x_{k+1}\right)=\left(\begin{array}{c}\arctan \left(\frac{2(q_{2,k+1}{q}_{3,k+1}+q_{0,k+1}{q}_{1,k+1})}{1-2(q_{1,k+1}^2+q_{2,k+1}^2)}\right)\\ \arctan (-2(q_{1,k+1}{q}_{3,k+1}-q_{0,k+1}{q}_{2,k+1}))\\ \arctan \left(\frac{2(q_{1,k+1}{q}_{2,k+1}+q_{0,k+1}{q}_{3,k+1})}{1-2(q_{2,k+1}^2+q_{3,k+1}^2)}\right)\end{array}\right)---\left(4\right)$

记r₁、r₂和r₃分别表示横滚角、俯仰角、偏航角，则$R\left(w\right)={\left(\begin{array}{ccc}R\left(w_{r_1}\right)& R\left(w_{r_2}\right)& R\left(w_{r_3}\right)\end{array}\right)}^T$为观测噪声函数向量，$w={\left(\begin{array}{ccc}{w}_{r_1}& w_{r_2}& w_{r_3}\end{array}\right)}^T$是四旋翼飞行器姿态的观测噪声变化向 量，并有

其为一个三角形可能性分布函数，其中$w_{r_l}\in [w_{a,r_l},w_{b,r_l}],w_{c,r_l}=(w_{a,r_l}+w_{b,r_l})/2;$

四旋翼飞行器四元数状态向量$x_k={\left(\begin{array}{cccc}{q}_{0,k}& q_{1,k}& q_{2,k}& q_{3,k}\end{array}\right)}^T$中的各元素与观测向 量$z_k={\left(\begin{array}{ccc}{r}_{1,k}& r_{2,k}& r_{3,k}\end{array}\right)}^T$各元素之间的转换关系函数如式(6)所示

$q_{i,k}=\frac{q_{i,k}^{\prime }}{\sqrt{q_{0,k}^{\prime 2}+q_{1,k}^{\prime 2}+q_{2,k}^{\prime 2}+q_{3,k}^{\prime 2}}}---\left(6\right)$

其中

$q_{0,k}^{\prime }=\cos \frac{r_{1,k}}{2}\cos \frac{r_{2,k}}{2}\cos \frac{r_{3,k}}{2}+\sin \frac{r_{1,k}}{2}\sin \frac{r_{2,k}}{2}\sin \frac{r_{3,k}}{2}---\left(7\right)$

$q_{1,k}^{\prime }=\sin \frac{r_{1,k}}{2}\cos \frac{r_{2,k}}{2}\cos \frac{r_{3,k}}{2}-\cos \frac{r_{1,k}}{2}\sin \frac{r_{2,k}}{2}\sin \frac{r_{3,k}}{2}---\left(8\right)$

$q_{2,k}^{\prime }=\cos \frac{r_{1,k}}{2}\sin \frac{r_{2,k}}{2}\cos \frac{r_{3,k}}{2}+\sin \frac{r_{1,k}}{2}\cos \frac{r_{2,k}}{2}\sin \frac{r_{3,k}}{2}---\left(9\right)$

$q_{3,k}^{\prime }=\cos \frac{r_{1,k}}{2}\cos \frac{r_{2,k}}{2}\sin \frac{r_{3,k}}{2}-\sin \frac{r_{1,k}}{2}\sin \frac{r_{2,k}}{2}\cos \frac{r_{3,k}}{2}---\left(10\right)$

（2）构造状态噪声函数向量Q(v)关于四元数状态向量x_k中元素q_i,k的证据 $(F_Q^{q_i},B_Q^{q_i}),i=\mathrm{0,1,2,3},$具体步骤如下：

（2-1）构造状态噪声函数向量Q(v)关于四元数状态向量x_k元素q_i,k的初始证 据其中${\bar{F}}_Q^{q_i}=\left\{\right[L_{{\alpha }_0}^{q_i-},R_{{\alpha }_0}^{q_i+}],[L_{{\alpha }_1}^{q_i-},R_{{\alpha }_1}^{q_i+}],...,[L_{{\alpha }_j}^{q_i-},R_{{\alpha }_j}^{q_i+}],...,[L_{{\alpha }_{p-1}}^{q_i-},R_{{\alpha }_{p-1}}^{q_i+}\left]\right\},$表示关于 k时刻四旋翼飞行器四元数状态向量$x_k={\left(\begin{array}{cccc}{q}_{0,k}& q_{1,k}& q_{2,k}& q_{3,k}\end{array}\right)}^T$中元素q_i,k(i=0,1,2,3) 的状态噪声区间集合，为中的第j个取值为区间的元素，j=0,1,…,p-1， p∈[3,5]，为它的左端点，为它的右端点，且有

$\left(\begin{array}{c}{L}_{{\alpha }_j}^{q_i-}=\underset{v_{q_i}}{\min }\left\{v_{q_i}\right|Q\left(v_{q_i}\right)\ge {\alpha }_j\}\\ R_{{\alpha }_j}^{q_i+}=\underset{v_{q_i}}{\max }\left\{v_{q_i}\right|Q\left(v_{q_i}\right)\ge {\alpha }_j\}\end{array}\right)---\left(11\right)$

其中α_j＝j/p，则有

$[L_{{\alpha }_0}^{q_i-},R_{{\alpha }_0}^{q_i+}]\supset [L_{{\alpha }_1}^{q_i-},R_{{\alpha }_1}^{q_i+}]\supset ...[L_{{\alpha }_j}^{q_i-},R_{{\alpha }_j}^{q_i+}]\supset ...\supset [L_{{\alpha }_{p-2}}^{q_i-},R_{{\alpha }_{p-2}}^{q_i+}]\supset [L_{{\alpha }_{p-1}}^{q_i-},R_{{\alpha }_{p-1}}^{q_i+}]$是中各区间元素的信度组成的集合，并有

${\bar{B}}_Q^{q_i}=\left\{{\bar{B}}_Q^{q_i}\right([L_{{\alpha }_0}^{q_i-},R_{{\alpha }_0}^{q_i+}]),{\bar{B}}_Q^{q_i}([L_{{\alpha }_1}^{q_i-},R_{{\alpha }_1}^{q_i+}]),...,{\bar{B}}_Q^{q_i}([L_{{\alpha }_j}^{q_i-},R_{{\alpha }_j}^{q_i+}]),...,{\bar{B}}_Q^{q_i}([L_{{\alpha }_{p-1}}^{q_i-},R_{{\alpha }_{p-1}}^{q_i+}\left]\right)\}$其元素的取值如式(12)所示

${\bar{B}}_Q^{q_i}\left(\right[L_{{\alpha }_j}^{q_i-},R_{{\alpha }_j}^{q_i+}\left]\right)=\left(\begin{array}{cc}{\alpha }_{j+1}& j=0\\ {\alpha }_{j+1}-{\alpha }_j& j=\mathrm{1,2},...p-2\\ 1-{\alpha }_j& j=p-1\end{array}\right)---\left(12\right)$

（2-2）对步骤（2-1）获取的初始证据分别进行折扣计算，获得状 态噪声函数向量Q(v)关于四元数状态向量x_k元素q_i,k的证据其中关于 q_i,k的状态噪声区间集合为

$F_Q^{q_i}=\left\{[L_{{\alpha }_0}^{q_i-},R_{{\alpha }_0}^{q_i+}],[L_{{\alpha }_1}^{q_i-},R_{{\alpha }_1}^{q_i+}],...,[L_{{\alpha }_j}^{q_i-},R_{{\alpha }_j}^{q_i+}],...,[L_{{\alpha }_{p-1}}^{q_i-},R_{{\alpha }_{p-1}}^{q_i+}],[{\lambda ·L}_{{\alpha }_0}^{q_i-},{\lambda ·R}_{{\alpha }_0}^{q_i+}]\right\},\lambda \in \left[\mathrm{10,100}\right]$为中各区间元素的信度组成的集合

$B_Q^{q_i}=\left\{B_Q^{q_i}\right([L_{{\alpha }_0}^{q_i-},R_{{\alpha }_0}^{q_i+}]),B_Q^{q_i}([L_{{\alpha }_1}^{q_i-},R_{{\alpha }_1}^{q_i+}]),...,B_Q^{q_i}([L_{{\alpha }_j}^{q_i-},R_{{\alpha }_j}^{q_i+}]),...,B_Q^{q_i}([L_{{\alpha }_{p-1}}^{q_i-},R_{{\alpha }_{p-1}}^{q_i+}]),B_Q^{q_i}([{\lambda ·L}_{{\alpha }_0}^{q_i-},{\lambda ·R}_{{\alpha }_0}^{q_i+}]\left)\right\}$

其中

$B_Q^{q_i}\left(\right[L_{{\alpha }_j}^{q_i-},R_{{\alpha }_j}^{q_i+}\left]\right)=(1-{ϵ}_Q){\bar{B}}_Q^{q_i}\left(\right[L_{{\alpha }_j}^{q_i-},R_{{\alpha }_j}^{q_i+}\left]\right),j=\mathrm{0,1}...,p-1,i=\mathrm{0,1,2,3}---\left(13\right)$

$B_Q^{q_i}\left(\right[{\lambda ·L}_{{\alpha }_0}^{q_i-},{\lambda ·R}_{{\alpha }_0}^{q_i+}])={ϵ}_Q---\left(14\right)$

这里折扣率ε_Q∈[0.03,0.08]；

为便于理解，这里举例说明，这里p=3，i=0,分别取值 $[L_{{\alpha }_0}^{q_i-},R_{{\alpha }_0}^{q_i+}]=[-\mathrm{0.005,0.005}],[L_{{\alpha }_1}^{q_i-},R_{{\alpha }_1}^{q_i+}]=[-0.010,0.010],$$[L_{{\alpha }_2}^{q_i-},R_{{\alpha }_2}^{q_i+}]=[-\mathrm{0.005,0.005}],{\alpha }_j=j/p,$即${\bar{B}}_Q^{q_i}\left(\right[L_{{\alpha }_0}^{q_i-},R_{{\alpha }_0}^{q_i+}])=1/3,$$B_Q^{q_i}\left([L_{{\alpha }_1}^{q_i-},R_{{\alpha }_1}^{q_i+}])=2/3-1/3=1/3,B_Q^{q_i}([L_{{\alpha }_2}^{q_i-},R_{{\alpha }_2}^{q_i+}\right])=1-2/3=1/3,$令ε_Q＝0.05，λ＝10， 则由式(13-14)得如表1所示

表1 的区间元素及信度赋值

（3）通过状态转移矩阵A获取k时刻关于向量x_k的元素q_i,k的预测证据 其中下标k+1|k表示利用k时刻的状态对k+1时刻的状态进 行预测，具体步骤如下：

（3-1）构建x_k元素q_i,k的状态估计证据具体步骤如下：

（a）当k=0时，取z_k的元素r_1,k r_2,k r_3,k分别带入式(7)-(10)得到

$q_{0,0|0}^{\prime }=\cos \frac{r_{1,0}}{2}\cos \frac{r_{2,0}}{2}\cos \frac{r_{3,0}}{2}+\sin \frac{r_{1,0}}{2}\sin \frac{r_{2,0}}{2}\sin \frac{r_{3,0}}{2}$

$q_{1,0|0}^{\prime }=\sin \frac{r_{1,0}}{2}\cos \frac{r_{2,0}}{2}\cos \frac{r_{3,0}}{2}-\cos \frac{r_{1,0}}{2}\sin \frac{r_{2,0}}{2}\sin \frac{r_{3,0}}{2}$

$q_{2,0|0}^{\prime }=\cos \frac{r_{1,0}}{2}\sin \frac{r_{2,0}}{2}\cos \frac{r_{3,0}}{2}+\sin \frac{r_{1,0}}{2}\cos \frac{r_{2,0}}{2}\sin \frac{r_{3,0}}{2}$

$q_{3,0|0}^{\prime }=\cos \frac{r_{1,0}}{2}\cos \frac{r_{2,0}}{2}\sin \frac{r_{3,0}}{2}-\sin \frac{r_{1,0}}{2}\sin \frac{r_{2,0}}{2}\cos \frac{r_{3,0}}{2}$

再利用式(6)得到

${\hat{q}}_{i,0|0}=\frac{q_{i,0|0}^{\prime }}{\sqrt{q_{\mathrm{0,0}|0}^{\prime 2}+q_{\mathrm{1,0}|0}^{\prime 2}+q_{\mathrm{2,0}|0}^{\prime 2}+q_{\mathrm{3,0}|0}^{\prime 2}}},i=\mathrm{0,1,2,3}$

由此得到状态估计向量${\hat{x}}_{0|0}={\left(\begin{array}{cccc}{\hat{q}}_{\mathrm{0,0}|0}& {\hat{q}}_{\mathrm{1,0}|0}& {\hat{q}}_{\mathrm{2,0}|0}& {\hat{q}}_{\mathrm{3,0}|0}\end{array}\right)}^T,$则状态估计证据中 的

$\left(\begin{array}{c}{F}_{0|0}^{q_i}=\left\{\right[L_{{\alpha }_0}^{q_i-}+{\hat{q}}_{i,0|0},R_{{\alpha }_0}^{q_i+}+{\hat{q}}_{i,0|0}],[L_{{\alpha }_1}^{q_i-}+{\hat{q}}_{i,0|0},R_{{\alpha }_1}^{q_i+}+{\hat{q}}_{i,0|0}],...,[L_{{\alpha }_j}^{q_i-}+{\hat{q}}_{i,0|0},R_{{\alpha }_j}^{q_i+}+{\hat{q}}_{i,0|0}],\\ ...,[L_{{\alpha }_{p-1}}^{q_i-}+{\hat{q}}_{i,0|0},R_{{\alpha }_{p-1}}^{q_i+}+{\hat{q}}_{i,0|0}],[\lambda ·L_{{\alpha }_0}^{q_i-}+{\hat{q}}_{i,0|0},\lambda ·R_{{\alpha }_0}^{q_i+}+{\hat{q}}_{i,0|0}]\}\end{array}\right)---\left(15\right)$

$B_{0|0}^{q_i}=B_Q^{q_i}---\left(16\right)$

亦即

$B_{0|0}^{q_i}\left(\right[L_{{\alpha }_j}^{q_i-}+{\hat{q}}_{i,0|0},R_{{\alpha }_j}^{q_i+}+{\hat{q}}_{i,0|0}\left]\right)=B_Q^{q_i}\left(\right[L_{{\alpha }_j}^{q_i-},R_{{\alpha }_j}^{q_i+}\left]\right),j=\mathrm{0,1}...,p-1,i=\mathrm{0,1,2,3}$

$B_{0|0}^{q_i}[\lambda ·L_{{\alpha }_0}^{q_i-}+{\hat{q}}_{i,0|0},\lambda ·R_{{\alpha }_0}^{q_i+}+{\hat{q}}_{i,0|0}]=B_Q^{q_i}\left(\right[{\lambda ·L}_{{\alpha }_0}^{q_i-},{\lambda ·R}_{{\alpha }_0}^{q_i+}\left]\right)$

（b）k≥1时，由递归计算得到状态估计向量则元素 q_i,k的状态估计证据为

$\left(\begin{array}{c}{F}_{k|k}^{q_i}=\left\{\right[L_{{\alpha }_0}^{q_i-}+{\hat{q}}_{i,k|k},R_{{\alpha }_0}^{q_i+}+{\hat{q}}_{i,k|k}],[L_{{\alpha }_1}^{q_i-}+{\hat{q}}_{i,k|k},R_{{\alpha }_1}^{q_i+}+{\hat{q}}_{i,k|k}],...,[L_{{\alpha }_j}^{q_i-}+{\hat{q}}_{i,k|k},R_{{\alpha }_j}^{q_i+}+{\hat{q}}_{i,k|k}],\\ ...,[L_{{\alpha }_{p-1}}^{q_i-}+{\hat{q}}_{i,k|k},R_{{\alpha }_{p-1}}^{q_i+}+{\hat{q}}_{i,k|k}],[\lambda ·L_{{\alpha }_0}^{q_i-}+{\hat{q}}_{i,k|k},\lambda ·R_{{\alpha }_0}^{q_i+}+{\hat{q}}_{i,k|k}]\}\end{array}\right)---\left(17\right)$

$B_{k|k}^{q_i}=B_Q^{q_i}---\left(18\right)$

亦即，

$B_{k|k}^{q_i}\left(\right[L_{{\alpha }_j}^{q_i-}+{\hat{q}}_{i,k|k},R_{{\alpha }_j}^{q_i+}+{\hat{q}}_{i,k|k}\left]\right)=B_Q^{q_i}\left(\right[L_{{\alpha }_j}^{q_i-},R_{{\alpha }_j}^{q_i+}\left]\right),j=\mathrm{0,1}...,p-1,i=\mathrm{0,1,2,3}$

$B_{k|k}^{q_i}[\lambda ·L_{{\alpha }_0}^{q_i-}+{\hat{q}}_{i,k|k},\lambda ·R_{{\alpha }_0}^{q_i+}+{\hat{q}}_{i,k|k}]=B_Q^{q_i}\left(\right[{\lambda ·L}_{{\alpha }_0}^{q_i-},{\lambda ·R}_{{\alpha }_0}^{q_i+}\left]\right)$

（3-2）通过状态转移矩阵A获取向量x_k的元素q_i,k的预测证据 $E_{k+1|k}^{q_i}=(F_{k+1|k}^{q_i},B_{k+1|k}^{q_i}),$其中

$\left(\begin{array}{c}{F}_{k+1|k}^{q_i}=\left\{\right[A_{i+1,i+1}(L_{{\alpha }_0}^{q_i-}+{\hat{q}}_{i,k|k}),A_{i+1,i+1}(R_{{\alpha }_0}^{q_i+}+{\hat{q}}_{i,k|k})],[A_{i+1,i+1}(L_{{\alpha }_1}^{q_i-}+{\hat{q}}_{i,k|k}),A_{i+1,i+1}(R_{{\alpha }_1}^{q_i+}+{\hat{q}}_{i,k|k})],...,\\ [A_{i+1,i+1}(L_{{\alpha }_j}^{q_i-}+{\hat{q}}_{i,k|k}),A_{i+1,i+1}(R_{{\alpha }_j}^{q_i+}+{\hat{q}}_{i,k|k})],...,[A_{i+1,i+1}(L_{{\alpha }_{p-1}}^{q_i-}+{\hat{q}}_{i,k|k}),\\ A_{i+1,i+1}(R_{{\alpha }_{p-1}}^{q_i+}+{\hat{q}}_{i,k|k})],[A_{i+1,i+1}(\lambda ·L_{{\alpha }_0}^{q_i-}+{\hat{q}}_{i,k|k}),A_{i+1,i+1}(\lambda ·R_{{\alpha }_0}^{q_i+}+{\hat{q}}_{i,k|k})\left]\right\}\end{array}\right)---\left(19\right)$

这里，A_i+1,i+1表示状态转移矩阵A中，第i+1行i+1列的元素，其中i＝0,1,2,3， 并有A_i+1,i+1=1，故并有即：

$B_{k+1|k}^{q_i}\left(\right[A_{i+1,i+1}(L_{{\alpha }_j}^{q_i-}+{\hat{q}}_{i,k|k}),A_{i+1,i+1}(R_{{\alpha }_j}^{q_i+}+{\hat{q}}_{i,k|k})\left]\right)=B_{k|k}^{q_i}\left(\right[L_{{\alpha }_j}^{q_i-}+{\hat{q}}_{i,k|k},{\lambda ·R}_{{\alpha }_j}^{q_i+}+{\hat{q}}_{i,k|k}\left]\right)---\left(20\right)$

$B_{k|k}^{q_i}\left(\right[A_{i+1,i+1}(\lambda ·L_{{\alpha }_0}^{q_i-}+{\hat{q}}_{i,k|k}),A_{i+1,i+1}(\lambda ·R_{{\alpha }_0}^{q_i+}+{\hat{q}}_{i,k|k})\left]\right)=B_{k|k}^{q_i}\left(\right[{\lambda ·L}_{{\alpha }_0}^{q_i-}+{\hat{q}}_{i,k|k},{\lambda ·R}_{{\alpha }_0}^{q_i+}+{\hat{q}}_{i,k|k}\left]\right)---\left(21\right)$

（4）通过观测方程获取k+1时刻关于四旋翼飞行器姿态的观测向量z_k+1的元 素r_l,k+1的观测预测证据具体步骤如下：

（4-1）通过步骤（3）得到状态预测证据之后，分别抽取 中的一个元素进行排列组合，共计产生(p+1)⁴个四元区间组，将 这些四元区间组作为式(4)的输入量，利用MATLAB2010a软件中的工具箱intlab， 分别得到关于r_l,k+1的(p+1)⁴个区间，它们组成的区间集合为

${\bar{F}}_{k+1|k}^{r_l}=\left\{\right[{\bar{L}}_1^{r_{l,k+1k}},{\bar{R}}_1^{r_{l,k+1k}}],[{\bar{L}}_2^{r_{l,k+1k}},{\bar{R}}_2^{r_{l,k+1k}}],...,[{\bar{L}}_{\tau }^{r_{l,k+1k}},{\bar{R}}_{\tau }^{r_{l,k+1k}}],...,[{\bar{L}}_{{(p+1)}^4}^{r_{l,k+1k}},{\bar{R}}_{{(p+1)}^4}^{r_{l,k+1k}}\left]\right\},\tau =\mathrm{1,2,3},...,{(p+1)}^4---\left(22\right)$

并得到式(22)中每个区间的信度赋值组成的信度集合为

${\bar{B}}_{k+1|k}^{r_l}=\left\{{\bar{B}}_{k+1|k}^{r_l}\right([{\bar{L}}_1^{r_{l,k}},{\bar{R}}_1^{r_{l,k}}]),{\bar{B}}_{k+1|k}^{r_l}([{\bar{L}}_2^{r_{l,k+1k}},{\bar{R}}_2^{r_{l,k+1k}}]),...,{\bar{B}}_{k+1|k}^{r_l}([{\bar{L}}_{\tau }^{r_{l,k+1k}},{\bar{R}}_{\tau }^{r_{l,k+1k}}],...,{\bar{B}}_{k+1|k}^{r_l}\left(\right[{\bar{L}}_{{(p+1)}^4}^{r_{l,k+1k}},{\bar{R}}_{{(p+1)}^4}^{r_{l,k+1k}}\left]\right)\}---\left(23\right)$

其中的为所对应式(4)输入的四元区间组中每个区 间信度赋值的乘积，由式(22)和(23)可以构成关于r_l,k+1的初始观测预测证据 ${\bar{E}}_{k+1|k}^{r_l}={(\bar{F}}_{k+1|k}^{r_l},{\bar{B}}_{k+1|k}^{r_l});$

为便于理解，这里举例说明，取

$F_{k+1|k}^{q_0}=\left\{\right[\mathrm{0.9499,1.0500}],[\mathrm{0.9849,1.0150}\left]\right\},B_{k+1|k}^{q_0}=\left\{\mathrm{0.15,0.85}\right\},$

$F_{k+1|k}^{q_1}=\left\{\right[-\mathrm{0.0449,0.552}],[-\mathrm{0.0099,0.0202}\left]\right\},B_{k+1|k}^{q_1}=\left\{\mathrm{0.15,0.85}\right\},$

$F_{k+1|k}^{q_2}=\left\{\right[-\mathrm{0.0398,0.0603}],[-\mathrm{0.0048,0.0253}\left]\right\},B_{k+1|k}^{q_2}=\left\{\mathrm{0.15,0.85}\right\},$

$F_{k+1|k}^{q_3}=\left\{\right[-\mathrm{0.0037,0.0145}],[-\mathrm{0.0002,0.0203}\left]\right\},B_{k+1|k}^{q_3}=\left\{\mathrm{0.15,0.85}\right\},$

分别抽取中的一个元素进行排列组合，共计产生16个四元区间组， 这里，选择r₁横滚角的观测预测证据为例，将这些四元区间组作为式(4)第一行函 数的输入量，利用MATLAB2010a软件中的工具箱intlab可得：

表2 观测预测初始证据的区间元素及信度赋值

（4-2）将步骤（4-1）所得的进行简化后得到元素r_l,k的观 测预测证据$E_{k+1|k}^{r_l}=(F_{k+1|k}^{r_l},B_{k+1|k}^{r_l}),$其中

$F_{k+1|k}^{r_l}=\left\{\right[L_1^{r_{l,k+1k}},R_1^{r_{l,k+1k}}],[L_2^{r_{l,k+1k}},R_2^{r_{l,k+1k}}]---\left(24\right)$

$B_{k+1|k}^{r_l}=\{B_{k+1|k}^{r_l}\left(\right[L_1^{r_{l,k}},R_1^{r_{l,k}}\left]\right),B_{k+1|k}^{r_l}\left(\right[L_2^{r_{l,k}},R_2^{r_{l,k}}\left]\right)\}---\left(25\right)$

式(24)中的为中信度赋值最大的那个区间，则式(25)中该区间的 信度赋值若有多个区间信度赋值相等且最 大，则将它们取并后构成将它们的信度相加后构成 式(24)中的为剩余各区间取并后得到的区间，且 这些区间的信度相加后构成

为便于理解，这里举例说明，由表2得到的初始证据化 简得$E_{k+1|k}^{r_l}=(F_{k+1|k}^{r_l},B_{k+1|k}^{r_l}):$

$[L_1^{r_{l,k+1k}},R_1^{r_{l,k+1k}}]=[-\mathrm{0.0540,0.0753}],B_{k+1|k}^{r_l}\left(\right[L_1^{r_{l,k+1k}},R_1^{r_{l,k+1k}}])=0.5220$

$[L_2^{r_{l,k+1k}},R_2^{r_{l,k+1k}}]=[-\mathrm{0.2376,0.2599}]\cup [-\mathrm{0.2222,0.2447}]\cup ...\cup [-\mathrm{0.0597,0.0809}]=[-\mathrm{0.2376,0.2599}],$$B_{k+1|k}^{r_l}\left(\right[L_2^{r_{l,k}},R_2^{r_{l,k}}]=0.4780;$这里，为表2中前15项区间元素取并后得 到的区间。

（5）利用Dempster组合规则求出k+1时刻观测域的融合证据 ${\hat{E}}_{k+1}^{r_l}=({\hat{F}}_{k+1}^{r_l},{\hat{B}}_{k+1}^{r_l})$具体步骤如下：

（5-1）构建z_k+1元素r_l,k+1的实时观测证据具体步骤如下

（a）构造观测噪声函数向量R(w)关于观测向量z_k+1元素r_l,k+1的初始证据 其中${\bar{F}}_R^{r_l}=\left\{\right[L_{{\alpha }_0}^{r_l-},R_{{\alpha }_0}^{r_l+}],[L_{{\alpha }_1}^{r_l-},R_{{\alpha }_1}^{r_l+}],...,[L_{{\alpha }_j}^{r_l-},R_{{\alpha }_j}^{r_l+}],...,[L_{{\alpha }_{p-1}}^{r_l-},R_{{\alpha }_{p-1}}^{r_l+}\left]\right\},$表示关于k 时刻四旋翼飞行器观测向量z_k中元素r_l,k的观测噪声区间集合，为中 的第j个取值为区间的元素，j=0,1,…,p-1，p∈[3,5]，为它的左端点，为 它的右端点，且有

$\left(\begin{array}{c}{L}_{{\alpha }_j}^{r_l-}=\underset{w_{q_i}}{\min }\left\{w_{r_l}\right|R\left(w_{r_l}\right)\ge {\alpha }_j\}\\ R_{{\alpha }_j}^{r_l-}=\underset{w_{r_l}}{\max }\left\{w_{r_l}\right|R\left(w_{r_l}\right)\ge {\alpha }_j\}\end{array}\right)---\left(26\right)$

其中α_j＝j/p，则有

$[L_{{\alpha }_0}^{r_l-},R_{{\alpha }_0}^{r_l+}]\supset [L_{{\alpha }_1}^{r_l-},R_{{\alpha }_1}^{r_l+}]\supset ...[L_{{\alpha }_j}^{r_l-},R_{{\alpha }_j}^{r_l+}]\supset ...\supset [L_{{\alpha }_{p-2}}^{r_l-},R_{{\alpha }_{p-2}}^{r_l+}]\supset [L_{{\alpha }_{p-1}}^{r_l-},R_{{\alpha }_{p-1}}^{r_l+}]$是中各区间元素的信度组成的集合，并有

${\bar{B}}_R^{r_l}=\left\{{\bar{B}}_R^{r_l}\right([L_{{\alpha }_0}^{r_l-},R_{{\alpha }_0}^{r_l+}]),{\bar{B}}_R^{r_l}([L_{{\alpha }_1}^{r_l-},R_{{\alpha }_1}^{r_l+}]),...,{\bar{B}}_R^{r_l}([L_{{\alpha }_j}^{r_l-},R_{{\alpha }_j}^{r_l+}]),...,{\bar{B}}_R^{r_l}([L_{{\alpha }_{p-1}}^{r_l-},R_{{\alpha }_{p-1}}^{r_l+}\left]\right)\}$其元素的取值如式(26)所示

${\bar{B}}_R^{r_l}\left(\right[L_{{\alpha }_j}^{r_l-},R_{{\alpha }_j}^{r_l+}\left]\right)=\left(\begin{array}{cc}{\alpha }_{j+1}& j=0\\ {\alpha }_{j+1}-{\alpha }_j& j=\mathrm{1,2},...p-2\\ 1-{\alpha }_j& j=p-1\end{array}\right)---\left(27\right)$

（b）对步骤（a）获取的初始证据分别进行折扣计算，获得观测噪 声向量函数R(w)关于观测向量z_k+1元素r_l,k+1的证据其中关于r_l,k+1的观测 噪声区间集合为

$F_R^{r_l}=\left\{[L_{{\alpha }_0}^{r_l-},R_{{\alpha }_0}^{r_l+}],[L_{{\alpha }_1}^{r_l-},R_{{\alpha }_1}^{r_l+}],...,[L_{{\alpha }_j}^{r_l-},R_{{\alpha }_j}^{r_l+}],...,[L_{{\alpha }_{p-1}}^{r_l-},R_{{\alpha }_{p-1}}^{r_l+}],[{\lambda ·L}_{{\alpha }_0}^{r_l-},{\lambda ·R}_{{\alpha }_0}^{r_l+}]\right\},\lambda \in \left[\mathrm{10,100}\right]$为中各区间元素的信度赋值组成的集合

$B_R^{r_l}=\left\{B_R^{r_l}\right([L_{{\alpha }_0}^{r_l-},R_{{\alpha }_0}^{r_l+}]),B_R^{r_l}([L_{{\alpha }_1}^{r_l-},R_{{\alpha }_1}^{r_l+}]),...,B_R^{r_l}([L_{{\alpha }_j}^{r_l-},R_{{\alpha }_j}^{r_l+}]),...,B_R^{r_l}([L_{{\alpha }_{p-1}}^{r_l-},R_{{\alpha }_{p-1}}^{r_l+}]),B_R^{r_l}([{\lambda ·L}_{{\alpha }_0}^{r_r-},{\lambda ·R}_{{\alpha }_0}^{r_l+}]\left)\right\}$

其中

$B_R^{r_l}\left(\right[L_{{\alpha }_j}^{r_l-},R_{{\alpha }_j}^{r_l+}\left]\right)=(1-{ϵ}_R){\bar{B}}_R^{r_l}\left(\right[L_{{\alpha }_j}^{r_l-},R_{{\alpha }_j}^{r_l+}\left]\right),j=\mathrm{0,1}...,p-1,i=\mathrm{0,1,2,3}---\left(28\right)$

$B_R^{r_l}\left(\right[{\lambda ·L}_{{\alpha }_0}^{r_l-},{\lambda ·R}_{{\alpha }_0}^{r_l+}])={ϵ}_R---\left(29\right)$

这里折扣率ε_R∈[0.03,0.08]；

为便于理解，这里举例说明，设定p=3，l=1，元素取值分别为 $[L_{{\alpha }_0}^{r_l-},R_{{\alpha }_0}^{r_l+}]=[-\mathrm{0.0275,0.0275}],[L_{{\alpha }_1}^{r_l-},R_{{\alpha }_1}^{r_l+}]=[-\mathrm{0.055,0.055}],[L_{{\alpha }_2}^{r_l-},R_{{\alpha }_2}^{r_l+}]=[-\mathrm{0.0825,0.0825}],{\alpha }_j=j/p,$则 ${\bar{B}}_R^{r_l}\left([L_{{\alpha }_0}^{r_l-},R_{{\alpha }_0}^{r_l+}])=1/3,{\bar{B}}_R^{r_l}\left(\right[L_{{\alpha }_1}^{r_l-},R_{{\alpha }_1}^{r_l+}])=2/3-1/3=1/3,{\bar{B}}_R^{r_1}([L_{{\alpha }_2}^{r_1-},R_{{\alpha }_2}^{r_1+}\right])=1-2/3=1/3,$令ε_R＝0.05，λ＝10， 则由式(26-29)得如表3所示：

表3 的区间元素及信度赋值

（c）当从陀螺仪观测到向量$z_{k+1}={\left(\begin{array}{ccc}{r}_{1,k+1}& r_{2,k+1}& r_{3,k+1}\end{array}\right)}^T,$则构建元素r_l,k+1的观测 证据$(F_{k+1}^{r_l},B_{k+1}^{r_l})$为

$\left(\begin{array}{c}{F}_{k+1}^{r_l}=\left\{\right[L_{{\alpha }_0}^{r_l-}+r_{l,k+1},R_{{\alpha }_0}^{r_l+}+r_{l,k+1}],[L_{{\alpha }_1}^{r_l-}+r_{l,k+1},R_{{\alpha }_1}^{r_l+}+r_{l,k+1}],...,[L_{{\alpha }_j}^{r_l-}+r_{l,k+1},R_{{\alpha }_j}^{r_l+}+r_{l,k+1}],\\ ...,[L_{{\alpha }_{p-1}}^{r_l-}+r_{l,k+1},R_{{\alpha }_{p-1}}^{r_l+}+r_{l,k+1}],[\lambda ·L_{{\alpha }_0}^{r_l-}+r_{l,k+1},\lambda ·R_{{\alpha }_0}^{r_l+}+r_{l,k+1}]\}\end{array}\right)---\left(30\right)$

$B_{k+1}^{r_l}=B_R^{r_l}---\left(31\right)$

亦即：

$B_{k+1}^{r_l}\left(\right[L_{{\alpha }_j}^{r_l-}+r_{l,k+1},R_{{\alpha }_j}^{r_l+}+r_{l,k+1}\left]\right)=B_R^{r_l}\left(\right[L_{{\alpha }_j}^{r_l-},R_{{\alpha }_j}^{r_l+}\left]\right),j=\mathrm{0,1}...,p-1,l=\mathrm{1,2,3}$

$B_{k+1}^{r_l}\left(\right[{\lambda ·L}_{{\alpha }_0}^{r_l-}+r_{l,k+1},{\lambda ·R}_{{\alpha }_0}^{r_l+}+r_{l,k+1}\left]\right)=B_R^{r_l}\left(\right[\lambda ·L_{{\alpha }_0}^{r_l-},{\lambda ·R}_{{\alpha }_0}^{r_l+}\left]\right)$

（5-2）将得到的观测证据与观测预测证据利用 Dempster组合规则进行证据融合，得到观测域的融合证据

令$[L_1^{r_{l.k+1k}},R_1^{r_{l,k+1k}}]=A_1,[L_2^{r_{l,k+1k}},R_2^{r_{l,k+1k}}]=A_2,$其信度赋值为$B_{k+1|k}^{r_l}\left(A_g\right),g=\mathrm{1,2},$令 $[L_{{\alpha }_0}^{r_l-}+{\hat{r}}_{l,k+1},R_{{\alpha }_0}^{r_l+}+{\hat{r}}_{l,k+1}]=C_1,[L_{{\alpha }_1}^{r_l-}+{\hat{r}}_{l,k+1},R_{{\alpha }_1}^{r_l+}+{\hat{r}}_{l,k+1}]=C_2,...,$$[L_{{\alpha }_j}^{r_l-}+{\hat{r}}_{l,k+1},R_{{\alpha }_j}^{r_l+}{\hat{r}}_{l,k+1}]=C_{j+1},...,[L_{{\alpha }_{p-1}}^{r_l-}+{\hat{r}}_{l,k+1},R_{{\alpha }_{p-1}}^{r_l+}+{\hat{r}}_{l,k+1}]=C_p,$$[{\lambda ·L}_{{\alpha }_0}^{r_l-}+{\hat{r}}_{l,k+1},{\lambda ·R}_{{\alpha }_0}^{r_l+}+{\hat{r}}_{l,k+1}]=C_{p+1},$其信度赋值为$B_{k+1}^{r_l}\left(C_h\right),h=\mathrm{1,2},...,p+1;$

利用Dempster组合规则将和组合后得到

其中ζ＝1,2,…,n，n≤2·(p+1)表示通过A_g∩C_h共计生成了n个不同的区间 $\left\{[L_1^{r_{l,k+1}},R_1^{r_{l,k+1}}],[L_2^{r_{l,k+1}},R_2^{r_{l,k+1}}],...,[L_{\zeta }^{r_{l,k+1}},R_{\zeta }^{r_{l,k+1}}],...,[L_n^{r_{l,k+1}},R_n^{r_{l,k+1}}]\right\},$则观测域的融合证据 ${\hat{E}}_{k+1}^{r_l}=({\hat{F}}_{k+1}^{r_l},{\hat{B}}_{k+1}^{r_l})$为

${\hat{F}}_{k+1}^{r_l}=\left\{[L_1^{r_{l,k+1}},R_1^{r_{l,k+1}}],[L_2^{r_{l,k+1}},R_2^{r_{l,k+1}}],...,[L_{\zeta }^{r_{l,k+1}},R_{\zeta }^{r_{l,k+1}}],...,[L_n^{r_{l,k+1}},R_n^{r_{l,k+1}}]\right\}$中对n个不同区间的信度赋值，可以由式(32)得到；

为便于理解，这里举例说明，具体数据见表4-6所示：

表4 观测预测证据的区间元素及信度赋值

表5 观测证据的区间元素及信度赋值

Dempster组合的具体方式如下：

X1=[-0.0574,0.0798]∩[-0.1853,0.1648]=[-0.0574,0.0798]，

X2=[-0.0574,0.0798]∩[-0.0628,0.0423]=[-0.0574,0.0423]，

X3=[-0.0574,0.0798]∩[-0.0453,0.0248]=[-0.0453,0.0248]，

X4=[-0.0574,0.0798]∩[-0.0278,0.0073]=[-0.0278,0.0073]，

X5=[-0.2936,0.3204]∩[-0.1853,0.1648]=[-0.1853,0.1648]，

X6=[-0.2936,0.3204]∩[-0.0628,0.0423]=[-0.0628,0.0423]，

X7=[-0.2936,0.3204]∩[-0.0453,0.0248]=[-0.0453,0.0248]，

X8=[-0.2936,0.3204]∩[-0.0278,0.0073]=[-0.0278,0.0073]，

这里X3＝X7,X4＝X8故

$[L_1^{r_{l,k+1k}},R_1^{r_{l,k+1k}}]=[-\mathrm{0.0574,0.0798}],{\hat{B}}_{k+1}^{r_1}[L_1^{r_{l,k+1}},R_1^{r_{l,k+1k}}]=0.0332;[L_2^{r_{l,k+1}},R_2^{r_{l,k+1}}]=[-\mathrm{0.0574,0.0423}],$${\hat{B}}_{k+1}^{r_1}[L_2^{r_{l,k+1}},R_2^{r_{l,k+1k}}]=0.2101;$

$[L_3^{r_{l,k+1k}},R_3^{r_{l,k+1k}}]=[-\mathrm{0.0453,0.0248}],{\hat{B}}_{k+1}^{r_1}[L_3^{r_{l,k+1}},R_3^{r_{l,k+1k}}]=0.2101+0.1066=0.3167;$

$[L_4^{r_{l,k+1k}},R_4^{r_{l,k+1k}}]=[-\mathrm{0.0278,0.0073}],{\hat{B}}_{k+1}^{r_1}[L_4^{r_{l,k+1}},R_4^{r_{l,k+1k}}]=0.2101+0.1066=0.3167;$

$[L_5^{r_{l,k+1k}},R_5^{r_{l,k+1k}}]=[-\mathrm{0.1853,0.1648}],{\hat{B}}_{k+1}^{r_1}[L_5^{r_{l,k+1}},R_5^{r_{l,k+1k}}]=0.0168;$

$[L_6^{r_{l,k+1k}},R_6^{r_{l,k+1k}}]=[-\mathrm{0.0628,0.0423}],{\hat{B}}_{k+1}^{r_1}[L_6^{r_{l,k+1}},R_6^{r_{l,k+1k}}]=0.1066;$

由此可得观测域的融合证据见表6所示：

表6 观测域融合证据的区间元素及信度赋值

（6）通过观测方程的逆运算获得k+1时刻关于向量x_k+1的元素q_i,k+1的状态 域的新证据具体步骤如下：

（6-1）由步骤（5）得到的观测域的融合证据之后，分别抽 取中的一个元素进行排列组合，共计产生n³个三元区间组，将这些 三元区间组分别作为式(7-10)的输入量，利用MATLAB2010a软件中的工具箱 intlab，得到关于q_i,k+1的n³个区间，它们组成的区间集合为

${\bar{F}}_{k+1}^{q_i}=\left\{\right[{\bar{L}}_1^{q_{i,k+1}},{\bar{R}}_1^{q_{i,k+1}}],[{\bar{L}}_2^{q_{i,k+1}},{\bar{R}}_2^{q_{i,k+1}}],...,[{\bar{L}}_{\zeta }^{q_{i,k+1}},{\bar{R}}_{\zeta }^{q_{i,k+1}}],...,[{\bar{L}}_{n^3}^{q_{i,k+1}},{\bar{R}}_{n^3}^{q_{i,k+1}}\left]\right\},\zeta =\mathrm{1,2,3},...,n^3---\left(33\right)$

并可得到式(33)中每个区间的信度赋值组成的信度集合为

${\bar{B}}_{k+1}^{q_i}=\left\{{\bar{B}}_{k+1}^{q_i}\right([{\bar{L}}_1^{q_{i,k+1}},{\bar{R}}_1^{q_{i,k+1}}]),{\bar{B}}_{k+1}^{q_i}([{\bar{L}}_2^{q_{i,k+1}},{\bar{R}}_2^{q_{i,k+1}}]),...,{\bar{B}}_{k+1}^{q_i}([{\bar{L}}_{\zeta }^{q_{i,k+1}},{\bar{R}}_{\zeta }^{q_{i,k+1}}]),...,{\bar{B}}_{k+1}^{q_i}([{\bar{L}}_{n^3}^{q_{i,k+1}},{\bar{R}}_{n^3}^{q_{i,k+1}}]\left)\right\}---\left(34\right)$

其中的为所对应式(7-10)输入的三元区间组中每个区 间信度赋值的乘积，由式(33)和(34)可以构成关于q_i,k+1的状态域的初始新证据 ${\bar{E}}_{k+1}^{q_i}=({\bar{F}}_{k+1}^{q_i},{\bar{B}}_{k+1}^{q_i});$

（6-2）将步骤（6-1）所得的进行简化后得到关于元素q_i,k+1的状态域的初始新证据其中

${\hat{F}}_{k+1}^{q_i}=\{[L_1^{q_{i,k+1}},R_1^{q_{i,k+1}}],[L_2^{q_{i,k+1}},R_2^{q_{i,k+1}}]\}---\left(35\right)$

${\hat{B}}_{k+1}^{q_i}=\{{\hat{B}}_{k+1}^{q_i}[L_1^{q_{i,k+1}},R_1^{q_{i,k+1}}],{\hat{B}}_{k+1}^{q_i}[L_2^{q_{i,k+1}},R_2^{q_{i,k+1}}]\}---\left(36\right)$

式(35)中的为中信度赋值最大的那个区间，则式(36)中该区间的信 度赋值若有多个区间信度赋值相等且最大， 则将它们取并后构成将它们的信度相加后构成式 (35)中的为剩余各区间取并后得到的区间，且这些区间的信度相加后 构成${\hat{B}}_{k+1}^{q_i}\left(\right[L_2^{q_{i,k+1}},R_2^{q_{i,k+1k}}]);$

（7）利用Dempster组合规则求出k+1时刻状态估计的融合证据

将步骤（6）中得到的关于向量x_k+1的元素q_i,k+1的状态域的新证据和步骤（3）中得到的关于向量x_k的元素q_i,k的预测证据利用 Dempster组合规则进行证据融合，得到k+1时刻状态估计的融合证据 $({\hat{F}}_{k+1|k+1}^{q_i},{\hat{B}}_{k+1|k+1}^{q_i});$

令$[L_1^{q_{i,k+1}},R_1^{q_{i,k+1}}]=G_1,[L_2^{q_{i,k+1}},R_2^{q_{i,k+1}}]=G_2,$其信度赋值为${\hat{B}}_{k+1}^{q_i}\left(G_g\right),g=\mathrm{1,2},$令 $[L_{{\alpha }_0}^{q_i-}+{\hat{q}}_{i,k|k},R_{{\alpha }_0}^{q_i+}+{\hat{q}}_{i,k|k}]=H_1,[L_{{\alpha }_1}^{q_i-}+{\hat{q}}_{i,k|k},R_{{\alpha }_1}^{q_i+}+{\hat{q}}_{i,k|k}]=H_2,...,$$[L_{{\alpha }_j}^{q_i-}+{\hat{q}}_{i,k|k},R_{{\alpha }_j}^{q_i+}+{\hat{q}}_{i,k|k}]=H_{j+1},...,[L_{{\alpha }_{p-1}}^{q_i-}+{\hat{q}}_{i,k|k},R_{{\alpha }_{p-1}}^{q_i+}+{\hat{q}}_{i,k|k}]=H_p,$$[{\lambda ·L}_{{\alpha }_0}^{q_i-}+{\hat{q}}_{i,k|k},{\lambda ·R}_{{\alpha }_0}^{q_i+}+{\hat{q}}_{i,k|k}]=H_{p+1},$其信度赋值为$B_{k+1|k}^{q_i}\left(H_h\right),h=\mathrm{1,2},...,p+1;$

利用Dempster组合规则将组合后得到

其中ξ＝1,2,…，m，m≤2·(p+1)表示通过G_g∩H_h共计生成m个不同的区间，则 状态估计的融合证据为

${\hat{F}}_{k+1|k+1}^{q_i}=\left\{[L_1^{q_{i,k+1}},R_1^{q_{i,k+1}}],[L_2^{q_{i,k+1}},R_2^{q_{i,k+1}}],...,[L_{\zeta }^{q_{i,k+1}},R_{\zeta }^{q_{i,k+1}}],...,[L_m^{q_{i,k+1}},R_m^{q_{i,k+1}}]\right\}$

中对m个不同区间的信度赋值，可以由式(37)得到；

（8）获得k+1时刻状态估计向量的元素的状态估计值：

${\hat{q}}_{i,k+1|k+1}=\frac{{\hat{q}}_{i,k+1|k+1}^{\prime }}{\sqrt{{\hat{q}}_{0,k+1|k+1}^{\prime 2}+{\hat{q}}_{1,k+1|k+1}^{\prime 2}+{\hat{q}}_{2,k+1|k+1}^{\prime 2}+{\hat{q}}_{3,k+1|k+1}^{\prime 2}}}---\left(38\right)$

其中

${\hat{q}}_{i,k+1|k+1}^{\prime }=\underset{\xi =1}{\overset{m}{\Sigma }}{\hat{B}}_{k+1|k+1}^{q_i}·\frac{1}{2}(L_{\xi }^{q_{i,k+1}}+R_{\xi }^{q_{i,k+1}})---\left(39\right)$

将获得的k+1时刻状态估计向量${\hat{x}}_{k+1|k+1}={[{\hat{q}}_{0,k+1|k+1},{\hat{q}}_{1,k+1|k+1},{\hat{q}}_{2,k+1|k+1},{\hat{q}}_{3,k+1|k+1}]}^T$带 入步骤（3）进行下一时刻算法迭代，便可递推得出每一时刻的状态估计。

为便于理解，这里举例说明，获得的k+1时刻q_i状态估计值如表7所示

表7 关于q_i融合证据的区间元素及信度赋值

经式(39)得：

$\left(\begin{array}{c}{q}_i=0.3366\times \frac{-0.0201+0.0300}{2}+0.5659\times \frac{-0.0301+0.0300}{2}+0.0364\times \frac{-0.0201+0.0339}{2}\\ +0.0611\times \frac{-0.0887+0.1000}{2}=0.0022\end{array}\right)$

以下结合附图，详细介绍本发明方法的实施例：

本发明方法的流程图如图1所示，核心部分是：对于状态噪声与观测噪声证 据的构建，以及通过利用MATLAB2010a软件中的工具箱intlab中区间映射实现 证据的传递，以及利用Dempster组合规则进行证据融合，然后将获得的融合证 据进行加权求和后得到最终的状态估计值，整个算法可以迭代运行。

以下结合四旋翼飞行器模型及采集到的观测数据，详细介绍本发明方法的各 个步骤，并通过实验结果验证本发明提出的基于证据融合的四旋翼飞行器姿态估 计方法优于Ghalia Nassreddine提出的经典的基于置信函数的前反向传播算法。

1、建立四旋翼飞行器姿态的状态方程与观测方程

x_k+1＝Ax_k+Q(v)

z_k+1＝h(x_k+1)+R(w)   k=0,1,2,3…

这里，x_k从k=0时刻状态开始，Q(v)与R(w)函数向量中的元素分别可以表 示为以下形式的三角形可能性分布函数：

2、构造状态噪声函数向量Q(v)关于四元数状态向量x_k中元素q_i,k的证据 $(F_Q^{q_i},B_Q^{q_i})$

这里p=3，分别取值$[L_{{\alpha }_0}^{q_i-},R_{{\alpha }_0}^{q_i+}]=[-\mathrm{0.010,0.010}],[L_{{\alpha }_1}^{q_i-},R_{{\alpha }_1}^{q_i+}]=[-\mathrm{0.020,0.020}],$$[L_{{\alpha }_2}^{q_i-},R_{{\alpha }_2}^{q_i+}]=[-\mathrm{0.030,0.030}],{\alpha }_j=j/p,$即${\bar{B}}_Q^{q_i}([L_{{\alpha }_0}^{q_i-},R_{{\alpha }_0}^{q_i+}])=1/3,$$B_Q^{q_i}\left(\right[L_{{\alpha }_1}^{q_i-},R_{{\alpha }_1}^{q_i+}\left]\right)=2/3-1/3=1/3,B_Q^{q_i}\left(\right[L_{{\alpha }_2}^{q_i-},R_{{\alpha }_2}^{q_i+}])=1-2/3=1/3,$令ε_Q＝0.05，λ＝10， 则由式(13-14)得如表8所示：

表8 的区间元素及信度赋值

3、通过状态转移矩阵A获取k时刻关于向量x_k的元素q_i,k的预测证据 这里给出k=0时刻开始的迭代过程。

当k=0时，由式(15-16)分别可以得到各元素的状态估计证据如表9所示：

表9 0时刻状态变量证据的区间元素及信度赋值

3-2）通过状态转移矩阵A获取向量x_k的元素q_i,k的预测证据当k=0时，由式(19-21)我们分别可以得到x_k中各元素的状态预测证据如表 10所示：

表10 0时刻状态预测证据的区间元素及信度赋值

4、通过观测方程获取k时刻关于四旋翼飞行器的观测向量z_k的元素r_l,k的观测 预测证据$E_{k+1|k}^{r_l}=(F_{k+1|k}^{r_l},B_{k+1|k}^{r_l})$

经式(22-25)简化后，得到观测预测证据如表11所示：

表11 观测预测证据的区间元素及信度赋值

5、利用Dempster组合规则求出k+1时刻观测域的融合证据

这里p=3，分别取值$[L_{{\alpha }_0}^{r_l-},R_{{\alpha }_0}^{r_l+}]=[-\mathrm{0.0275,0.0275}],$$[L_{{\alpha }_1}^{r_l-},R_{{\alpha }_1}^{r_l+}]=[-\mathrm{0.055,0.055}],[L_{{\alpha }_2}^{r_l-},R_{{\alpha }_2}^{r_l+}]=[-\mathrm{0.0825,0.0825}],{\alpha }_j=j/p,$即 ${\bar{B}}_R^{r_l}\left([L_{{\alpha }_0}^{r_l-},R_{{\alpha }_0}^{r_l+}])=1/3,{\bar{B}}_R^{r_l}\left(\right[L_{{\alpha }_1}^{r_l-},R_{{\alpha }_1}^{r_l+}])=2/3-1/3=1/3,{\bar{B}}_R^{r_1}([L_{{\alpha }_2}^{r_1-},R_{{\alpha }_2}^{r_1+}\right])=1-2/3=1/3,$令 ε_R＝0.05，λ＝10，则由式(26-29)得如表12所示：

表12 的区间元素及信度赋值

当k=0时，经式(30-31)可以得到元素r_l,k+1的观测证据如表13所示：

表13 观测证据的区间元素及信度赋值

经式(32)利用Dempster组合规则将观测证据与观测预测证据进行证据融合 可以得到观测域的融合证据如表14-16所示：

表14 关于r₁的观测域融合证据的区间元素及信度赋值

表15 关于r₂的观测域融合证据的区间元素及信度赋值

表16 关于r₃的观测域融合证据的区间元素及信度赋值

6、通过观测方程的逆运算获得k+1时刻关于向量x_k+1的元素q_i,k+1的状态域 的新证据${\hat{E}}_{k+1}^{q_i}=({\hat{F}}_{k+1}^{q_i},{\hat{B}}_{k+1}^{q_i})$

经式(33-36)简化后，得到状态域的新证据如表17所示：

表17 状态域新证据的区间元素及信度赋值

7、利用Dempster组合规则求出k+1时刻状态估计的融合证据

经式(37)利用Dempster组合规则将状态域新证据与状态预测证据进行证据 融合可以得到状态域的融合证据如表18-21所示：

表18 关于q₀的状态域融合证据的区间元素及信度赋值

表19 关于q₁的状态域融合证据的区间元素及信度赋值

表20 关于q₂的状态域融合证据的区间元素及信度赋值

表21 关于q₃的状态域融合证据的区间元素及信度赋值

8、由表18-21经式(38-39)获得k+1时刻状态估计向量的元素的 状态估计值如表22所示：

表22 状态变量估计值

按照此方法，将获得的k+1时刻状态估计向量 ${\hat{x}}_{k+1|k+1}={[{\hat{q}}_{0,k+1|k+1},{\hat{q}}_{1,k+1|k+1},{\hat{q}}_{2,k+1|k+1},{\hat{q}}_{3,k+1|k+1}]}^T$带入步骤（3）进行下面每一时刻的算法 迭代，最终得到每个时刻的状态估计值这里将得到的每个时刻的状态估计 值代入式(4)得到用于四旋翼姿态控制的各姿态角的估计值如图2所示，其中 理想的真实值在每个时刻表现在四旋翼飞行器姿态的角度值均为0，用“—”表 示(k=0,1,2,…,20)，本发明方法给出的估计值用“--*--”表示，Ghalia Nassreddine 经典算法给出的估计值用“-◇-”表示，陀螺仪测得的观测值用“-+-”表示。 图3给出本发明方法、Ghalia Nassreddine经典算法和直接观测之间绝对误差的比 较。表23给出了三种方法在二十一步的估计中绝对误差的均值，可以看出本发 明方法的估计精度要高于其他方法。

表23 四旋翼飞行器姿态各角度估计的绝对误差