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

Abstract

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

Description

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

技术领域

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

背景技术

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

根据飞行器的不同应用场景和性能要求，可采用不同的姿态测量***及估计方法，但其本质都是以非线性滤波为基础。目前广泛使用的非线性滤波方法是扩展卡尔曼滤波(ExtendedKalmanFilter，EKF)和无迹卡尔曼滤波(UnscentedKalmanFilter，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}\∈[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}^{\′}}{\sqrt{q_{0,k}^{\′2}+q_{1,k}^{\′2}+q_{2,k}^{\′2}+q_{3,k}^{\′2}}}---\left(6\right)$

其中

$q_{0,k}^{\′}=\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}^{\′}=\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}^{\′}=\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}^{\′}=\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的初始证据其中 ${\stackrel{\‾}{F}}_Q^{q_i}=\left\{\right[L_{{\mathrm{\α}}_0}^{q_i-},R_{{\mathrm{\α}}_0}^{q_i+}],[L_{{\mathrm{\α}}_1}^{q_i-},R_{{\mathrm{\α}}_1}^{q_i+}],...,[L_{{\mathrm{\α}}_j}^{q_i-},R_{{\mathrm{\α}}_j}^{q_i+}],...,[L_{{\mathrm{\α}}_{p-1}}^{q_i-},R_{{\mathrm{\α}}_{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]，为它的左端点，为它的右端点，且有

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

其中α_j＝j/p，则有

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

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

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

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

${\stackrel{\‾}{B}}_Q^{q_i}\left(\right[L_{{\mathrm{\α}}_j}^{q_i-},R_{{\mathrm{\α}}_j}^{q_i+}\left]\right)=\left\{\begin{array}{cc}{\mathrm{\α}}_{j+1}& j=0\\ {\mathrm{\α}}_{j+1}-{\mathrm{\α}}_j& j=\mathrm{1,2},...p-2\\ 1-{\mathrm{\α}}_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_{{\mathrm{\α}}_0}^{q_i-},R_{{\mathrm{\α}}_0}^{q_i+}],[L_{{\mathrm{\α}}_1}^{q_i-},R_{{\mathrm{\α}}_1}^{q_i+}],...,[L_{{\mathrm{\α}}_j}^{q_i-},R_{{\mathrm{\α}}_j}^{q_i+}],...,[L_{{\mathrm{\α}}_{p-1}}^{q_i-},R_{{\mathrm{\α}}_{p-1}}^{q_i+}],[{\mathrm{\λ}\·L}_{{\mathrm{\α}}_0}^{q_i-},{\mathrm{\λ}\·R}_{{\mathrm{\α}}_0}^{q_i+}]\right\},\mathrm{\λ}\∈\left[\mathrm{10,100}\right]$ 为中各区间元素的信度组成的集合

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

其中

$B_Q^{q_i}\left(\right[L_{{\mathrm{\α}}_j}^{q_i-},R_{{\mathrm{\α}}_j}^{q_i+}\left]\right)=(1-{\mathrm{\ϵ}}_Q){\stackrel{\‾}{B}}_Q^{q_i}\left(\right[L_{{\mathrm{\α}}_j}^{q_i-},R_{{\mathrm{\α}}_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[{\mathrm{\λ}\·L}_{{\mathrm{\α}}_0}^{q_i-},{\mathrm{\λ}\·R}_{{\mathrm{\α}}_0}^{q_i+}])={\mathrm{\ϵ}}_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,kr_2,kr_3,k分别带入式(7)-(10)得到

$q_{0,0|0}^{\′}=\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}^{\′}=\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}^{\′}=\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}^{\′}=\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}^{\′}}{\sqrt{q_{\mathrm{0,0}|0}^{\′2}+q_{\mathrm{1,0}|0}^{\′2}+q_{\mathrm{2,0}|0}^{\′2}+q_{\mathrm{3,0}|0}^{\′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,$ 则状态估计证据中的

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

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

亦即

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

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

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

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

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

亦即

$B_{k|k}^{q_i}\left(\right[L_{{\mathrm{\α}}_j}^{q_i-}+{\hat{q}}_{i,k|k},R_{{\mathrm{\α}}_j}^{q_i+}+{\hat{q}}_{i,k|k}\left]\right)=B_Q^{q_i}\left(\right[L_{{\mathrm{\α}}_j}^{q_i-},R_{{\mathrm{\α}}_j}^{q_i+}\left]\right),j=\mathrm{0,1}...,p-1,i=\mathrm{0,1,2,3}$ $B_{k|k}^{q_i}[\mathrm{\λ}\·L_{{\mathrm{\α}}_0}^{q_i-}+{\hat{q}}_{i,k|k},\mathrm{\λ}\·R_{{\mathrm{\α}}_0}^{q_i+}+{\hat{q}}_{i,k|k}]=B_Q^{q_i}\left(\right[{\mathrm{\λ}\·L}_{{\mathrm{\α}}_0}^{q_i-},{\mathrm{\λ}\·R}_{{\mathrm{\α}}_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}),$ 其中

$\begin{array}{c}{F}_{k+1|k}^{q_i}=\left\{\right[A_{i+1,i+1}(L_{{\mathrm{\α}}_0}^{q_i-}+{\hat{q}}_{i,k|k}),A_{i+1,i+1}(R_{{\mathrm{\α}}_0}^{q_i+}+{\hat{q}}_{i,k|k})],[A_{i+1,i+1}(L_{{\mathrm{\α}}_1}^{q_i-}+{\hat{q}}_{i,k|k}),A_{i+1,i+1}(R_{{\mathrm{\α}}_1}^{q_i+}+{\hat{q}}_{i,k|k})],...,\\ [A_{i+1,i+1}(L_{{\mathrm{\α}}_j}^{q_i-}+{\hat{q}}_{i,k|k}),A_{i+1,i+1}(R_{{\mathrm{\α}}_j}^{q_i+}+{\hat{q}}_{i,k|k})],...,[A_{i+1,i+1}(L_{{\mathrm{\α}}_{p-1}}^{q_i-}+{\hat{q}}_{i,k|k}),\\ A_{i+1,i+1}(R_{{\mathrm{\α}}_{p-1}}^{q_i+}+{\hat{q}}_{i,k|k})],[A_{i+1,i+1}(\mathrm{\λ}\·L_{{\mathrm{\α}}_0}^{q_i-}+{\hat{q}}_{i,k|k}),A_{i+1,i+1}(\mathrm{\λ}\·R_{{\mathrm{\α}}_0}^{q_i+}+{\hat{q}}_{i,k|k})\left]\right\}\end{array}---\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_{{\mathrm{\α}}_j}^{q_i-}+{\hat{q}}_{i,k|k}),A_{i+1,i+1}(R_{{\mathrm{\α}}_j}^{q_i+}+{\hat{q}}_{i,k|k})\left]\right)=B_{k|k}^{q_i}\left(\right[L_{{\mathrm{\α}}_j}^{q_i-}+{\hat{q}}_{i,k|k},{\mathrm{\λ}\·R}_{{\mathrm{\α}}_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}(\mathrm{\λ}\·L_{{\mathrm{\α}}_0}^{q_i-}+{\hat{q}}_{i,k|k}),A_{i+1,i+1}(\mathrm{\λ}\·R_{{\mathrm{\α}}_0}^{q_i+}+{\hat{q}}_{i,k|k})\left]\right)=B_{k|k}^{q_i}\left(\right[{\mathrm{\λ}\·L}_{{\mathrm{\α}}_0}^{q_i-}+{\hat{q}}_{i,k|k},{\mathrm{\λ}\·R}_{{\mathrm{\α}}_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)⁴个区间，它们组成的区间集合为

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

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

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

其中的为所对应式(4)输入的四元区间组中每个区间信度赋值的乘积，由式(22)和(23)可以构成关于r_l,k+1的初始观测预测证据 ${\stackrel{\‾}{E}}_{k+1|k}^{r_l}={(\stackrel{\‾}{F}}_{k+1|k}^{r_l},{\stackrel{\‾}{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的初始证据其中 ${\stackrel{\‾}{F}}_R^{r_l}=\left\{\right[L_{{\mathrm{\α}}_0}^{r_l-},R_{{\mathrm{\α}}_0}^{r_l+}],[L_{{\mathrm{\α}}_1}^{r_l-},R_{{\mathrm{\α}}_1}^{r_l+}],...,[L_{{\mathrm{\α}}_j}^{r_l-},R_{{\mathrm{\α}}_j}^{r_l+}],...,[L_{{\mathrm{\α}}_{p-1}}^{r_l-},R_{{\mathrm{\α}}_{p-1}}^{r_l+}\left]\right\},$ 表示关于k时刻四旋翼飞行器姿态观测向量z_k中元素r_l,k的观测噪声区间集合，为中的第j个取值为区间的元素，j=0,1,…,p-1，p∈[3,5]，为它的左端点，为它的右端点，且有

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

其中α_j＝j/p，则有

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

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

${\stackrel{\‾}{B}}_R^{r_l}\left(\right[L_{{\mathrm{\α}}_j}^{r_l-},R_{{\mathrm{\α}}_j}^{r_l+}\left]\right)=\left\{\begin{array}{cc}{\mathrm{\α}}_{j+1}& j=0\\ {\mathrm{\α}}_{j+1}-{\mathrm{\α}}_j& j=\mathrm{1,2},...p-2\\ 1-{\mathrm{\α}}_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_{{\mathrm{\α}}_0}^{r_l-},R_{{\mathrm{\α}}_0}^{r_l+}],[L_{{\mathrm{\α}}_1}^{r_l-},R_{{\mathrm{\α}}_1}^{r_l+}],...,[L_{{\mathrm{\α}}_j}^{r_l-},R_{{\mathrm{\α}}_j}^{r_l+}],...,[L_{{\mathrm{\α}}_{p-1}}^{r_l-},R_{{\mathrm{\α}}_{p-1}}^{r_l+}],[{\mathrm{\λ}\·L}_{{\mathrm{\α}}_0}^{r_l-},{\mathrm{\λ}\·R}_{{\mathrm{\α}}_0}^{r_l+}]\right\},\mathrm{\λ}\∈\left[\mathrm{10,100}\right]$ 为中各区间元素的信度赋值组成的集合

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

其中

$B_R^{r_l}\left(\right[L_{{\mathrm{\α}}_j}^{r_l-},R_{{\mathrm{\α}}_j}^{r_l+}\left]\right)=(1-{\mathrm{\ϵ}}_R){\stackrel{\‾}{B}}_R^{r_l}\left(\right[L_{{\mathrm{\α}}_j}^{r_l-},R_{{\mathrm{\α}}_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[{\mathrm{\λ}\·L}_{{\mathrm{\α}}_0}^{r_l-},{\mathrm{\λ}\·R}_{{\mathrm{\α}}_0}^{r_l+}])={\mathrm{\ϵ}}_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})$ 为

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

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

亦即：

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

$B_{k+1}^{r_l}\left(\right[{\mathrm{\λ}\·L}_{{\mathrm{\α}}_0}^{r_l-}+r_{l,k+1},{\mathrm{\λ}\·R}_{{\mathrm{\α}}_0}^{r_l+}+r_{l,k+1}\left]\right)=B_R^{r_l}\left(\right[\mathrm{\λ}\·L_{{\mathrm{\α}}_0}^{r_l-},{\mathrm{\λ}\·R}_{{\mathrm{\α}}_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_{{\mathrm{\α}}_0}^{r_l-}+{\hat{r}}_{l,k+1},R_{{\mathrm{\α}}_0}^{r_l+}+{\hat{r}}_{l,k+1}]=C_1,[L_{{\mathrm{\α}}_1}^{r_l-}+{\hat{r}}_{l,k+1},R_{{\mathrm{\α}}_1}^{r_l+}+{\hat{r}}_{l,k+1}]=C_2,...,$ $[L_{{\mathrm{\α}}_j}^{r_l-}+{\hat{r}}_{l,k+1},R_{{\mathrm{\α}}_j}^{r_l+}+{\hat{r}}_{l,k+1}]=C_{j+1},...,[L_{{\mathrm{\α}}_{p-1}}^{r_l-}+{\hat{r}}_{l,k+1},R_{{\mathrm{\α}}_{p-1}}^{r_l+}+{\hat{r}}_{l,k+1}]=C_p,$ $[{\mathrm{\λ}\·L}_{{\mathrm{\α}}_0}^{r_l-}+{\hat{r}}_{l,k+1},{\mathrm{\λ}\·R}_{{\mathrm{\α}}_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_{\mathrm{\ζ}}^{r_{l,k+1}},R_{\mathrm{\ζ}}^{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_{\mathrm{\ζ}}^{r_{l,k+1}},R_{\mathrm{\ζ}}^{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³个区间，它们组成的区间集合为

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

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

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

其中的为所对应式(7-10)输入的三元区间组中每个区间信度赋值的乘积，由式(33)和(34)可以构成关于q_i,k+1的状态域的初始新证据 ${\stackrel{\‾}{E}}_{k+1}^{q_i}=({\stackrel{\‾}{F}}_{k+1}^{q_i},{\stackrel{\‾}{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_{{\mathrm{\α}}_0}^{q_i-}+{\hat{q}}_{i,k|k},R_{{\mathrm{\α}}_0}^{q_i+}+{\hat{q}}_{i,k|k}]=H_1,[L_{{\mathrm{\α}}_1}^{q_i-}+{\hat{q}}_{i,k|k},R_{{\mathrm{\α}}_1}^{q_i+}+{\hat{q}}_{i,k|k}]=H_2,...,$ $[L_{{\mathrm{\α}}_j}^{q_i-}+{\hat{q}}_{i,k|k},R_{{\mathrm{\α}}_j}^{q_i+}+{\hat{q}}_{i,k|k}]=H_{j+1},...,[L_{{\mathrm{\α}}_{p-1}}^{q_i-}+{\hat{q}}_{i,k|k},R_{{\mathrm{\α}}_{p-1}}^{q_i+}+{\hat{q}}_{i,k|k}]=H_p,$ $[{\mathrm{\λ}\·L}_{{\mathrm{\α}}_0}^{q_i-}+{\hat{q}}_{i,k|k},{\mathrm{\λ}\·R}_{{\mathrm{\α}}_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_{\mathrm{\ζ}}^{q_{i,k+1}},R_{\mathrm{\ζ}}^{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}^{\′}}{\sqrt{{\hat{q}}_{0,k+1|k+1}^{\′2}+{\hat{q}}_{1,k+1|k+1}^{\′2}+{\hat{q}}_{2,k+1|k+1}^{\′2}+{\hat{q}}_{3,k+1|k+1}^{\′2}}}---\left(38\right)$

其中，

${\hat{q}}_{i,k+1|k+1}^{\′}=\underset{\mathrm{\ξ}=1}{\overset{m}{\mathrm{\Σ}}}{\hat{B}}_{k+1|k+1}^{q_i}\·\frac{1}{2}(L_{\mathrm{\ξ}}^{q_{i,k+1}}+R_{\mathrm{\ξ}}^{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}\∈[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}^{\′}}{\sqrt{q_{0,k}^{\′2}+q_{1,k}^{\′2}+q_{2,k}^{\′2}+q_{3,k}^{\′2}}}---\left(6\right)$

其中

$q_{0,k}^{\′}=\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}^{\′}=\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}^{\′}=\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}^{\′}=\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的初始证据其中 ${\stackrel{\‾}{F}}_Q^{q_i}=\left\{\right[L_{{\mathrm{\α}}_0}^{q_i-},R_{{\mathrm{\α}}_0}^{q_i+}],[L_{{\mathrm{\α}}_1}^{q_i-},R_{{\mathrm{\α}}_1}^{q_i+}],...,[L_{{\mathrm{\α}}_j}^{q_i-},R_{{\mathrm{\α}}_j}^{q_i+}],...,[L_{{\mathrm{\α}}_{p-1}}^{q_i-},R_{{\mathrm{\α}}_{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]，为它的左端点，为它的右端点，且有

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

其中α_j＝j/p，则有

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

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

${\stackrel{\‾}{B}}_Q^{q_i}\left(\right[L_{{\mathrm{\α}}_j}^{q_i-},R_{{\mathrm{\α}}_j}^{q_i+}\left]\right)=\left\{\begin{array}{cc}{\mathrm{\α}}_{j+1}& j=0\\ {\mathrm{\α}}_{j+1}-{\mathrm{\α}}_j& j=\mathrm{1,2},...p-2\\ 1-{\mathrm{\α}}_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_{{\mathrm{\α}}_0}^{q_i-},R_{{\mathrm{\α}}_0}^{q_i+}],[L_{{\mathrm{\α}}_1}^{q_i-},R_{{\mathrm{\α}}_1}^{q_i+}],...,[L_{{\mathrm{\α}}_j}^{q_i-},R_{{\mathrm{\α}}_j}^{q_i+}],...,[L_{{\mathrm{\α}}_{p-1}}^{q_i-},R_{{\mathrm{\α}}_{p-1}}^{q_i+}],[{\mathrm{\λ}\·L}_{{\mathrm{\α}}_0}^{q_i-},{\mathrm{\λ}\·R}_{{\mathrm{\α}}_0}^{q_i+}]\right\},\mathrm{\λ}\∈\left[\mathrm{10,100}\right]$ 为中各区间元素的信度组成的集合

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

其中

$B_Q^{q_i}\left(\right[L_{{\mathrm{\α}}_j}^{q_i-},R_{{\mathrm{\α}}_j}^{q_i+}\left]\right)=(1-{\mathrm{\ϵ}}_Q){\stackrel{\‾}{B}}_Q^{q_i}\left(\right[L_{{\mathrm{\α}}_j}^{q_i-},R_{{\mathrm{\α}}_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[{\mathrm{\λ}\·L}_{{\mathrm{\α}}_0}^{q_i-},{\mathrm{\λ}\·R}_{{\mathrm{\α}}_0}^{q_i+}])={\mathrm{\ϵ}}_Q---\left(14\right)$

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

为便于理解，这里举例说明，这里p=3，i=0,分别取值 $[L_{{\mathrm{\α}}_0}^{q_i-},R_{{\mathrm{\α}}_0}^{q_i+}]=[-\mathrm{0.005,0.005}],[L_{{\mathrm{\α}}_1}^{q_i-},R_{{\mathrm{\α}}_1}^{q_i+}]=[-0.010,0.010],$ $[L_{{\mathrm{\α}}_2}^{q_i-},R_{{\mathrm{\α}}_2}^{q_i+}]=[-\mathrm{0.005,0.005}],{\mathrm{\α}}_j=j/p,$ 即 ${\stackrel{\‾}{B}}_Q^{q_i}\left(\right[L_{{\mathrm{\α}}_0}^{q_i-},R_{{\mathrm{\α}}_0}^{q_i+}])=1/3,$ $B_Q^{q_i}\left([L_{{\mathrm{\α}}_1}^{q_i-},R_{{\mathrm{\α}}_1}^{q_i+}])=2/3-1/3=1/3,B_Q^{q_i}([L_{{\mathrm{\α}}_2}^{q_i-},R_{{\mathrm{\α}}_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,kr_2,kr_3,k分别带入式(7)-(10)得到

$q_{0,0|0}^{\′}=\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}^{\′}=\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}^{\′}=\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}^{\′}=\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}^{\′}}{\sqrt{q_{\mathrm{0,0}|0}^{\′2}+q_{\mathrm{1,0}|0}^{\′2}+q_{\mathrm{2,0}|0}^{\′2}+q_{\mathrm{3,0}|0}^{\′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,$ 则状态估计证据中的

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

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

亦即

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

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

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

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

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

亦即，

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

$B_{k|k}^{q_i}[\mathrm{\λ}\·L_{{\mathrm{\α}}_0}^{q_i-}+{\hat{q}}_{i,k|k},\mathrm{\λ}\·R_{{\mathrm{\α}}_0}^{q_i+}+{\hat{q}}_{i,k|k}]=B_Q^{q_i}\left(\right[{\mathrm{\λ}\·L}_{{\mathrm{\α}}_0}^{q_i-},{\mathrm{\λ}\·R}_{{\mathrm{\α}}_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}),$ 其中

$\begin{array}{c}{F}_{k+1|k}^{q_i}=\left\{\right[A_{i+1,i+1}(L_{{\mathrm{\α}}_0}^{q_i-}+{\hat{q}}_{i,k|k}),A_{i+1,i+1}(R_{{\mathrm{\α}}_0}^{q_i+}+{\hat{q}}_{i,k|k})],[A_{i+1,i+1}(L_{{\mathrm{\α}}_1}^{q_i-}+{\hat{q}}_{i,k|k}),A_{i+1,i+1}(R_{{\mathrm{\α}}_1}^{q_i+}+{\hat{q}}_{i,k|k})],...,\\ [A_{i+1,i+1}(L_{{\mathrm{\α}}_j}^{q_i-}+{\hat{q}}_{i,k|k}),A_{i+1,i+1}(R_{{\mathrm{\α}}_j}^{q_i+}+{\hat{q}}_{i,k|k})],...,[A_{i+1,i+1}(L_{{\mathrm{\α}}_{p-1}}^{q_i-}+{\hat{q}}_{i,k|k}),\\ A_{i+1,i+1}(R_{{\mathrm{\α}}_{p-1}}^{q_i+}+{\hat{q}}_{i,k|k})],[A_{i+1,i+1}(\mathrm{\λ}\·L_{{\mathrm{\α}}_0}^{q_i-}+{\hat{q}}_{i,k|k}),A_{i+1,i+1}(\mathrm{\λ}\·R_{{\mathrm{\α}}_0}^{q_i+}+{\hat{q}}_{i,k|k})\left]\right\}\end{array}---\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_{{\mathrm{\α}}_j}^{q_i-}+{\hat{q}}_{i,k|k}),A_{i+1,i+1}(R_{{\mathrm{\α}}_j}^{q_i+}+{\hat{q}}_{i,k|k})\left]\right)=B_{k|k}^{q_i}\left(\right[L_{{\mathrm{\α}}_j}^{q_i-}+{\hat{q}}_{i,k|k},{\mathrm{\λ}\·R}_{{\mathrm{\α}}_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}(\mathrm{\λ}\·L_{{\mathrm{\α}}_0}^{q_i-}+{\hat{q}}_{i,k|k}),A_{i+1,i+1}(\mathrm{\λ}\·R_{{\mathrm{\α}}_0}^{q_i+}+{\hat{q}}_{i,k|k})\left]\right)=B_{k|k}^{q_i}\left(\right[{\mathrm{\λ}\·L}_{{\mathrm{\α}}_0}^{q_i-}+{\hat{q}}_{i,k|k},{\mathrm{\λ}\·R}_{{\mathrm{\α}}_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)⁴个区间，它们组成的区间集合为

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

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

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

其中的为所对应式(4)输入的四元区间组中每个区间信度赋值的乘积，由式(22)和(23)可以构成关于r_l,k+1的初始观测预测证据 ${\stackrel{\‾}{E}}_{k+1|k}^{r_l}={(\stackrel{\‾}{F}}_{k+1|k}^{r_l},{\stackrel{\‾}{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}]\∪[-\mathrm{0.2222,0.2447}]\∪...\∪[-\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的初始证据其中 ${\stackrel{\‾}{F}}_R^{r_l}=\left\{\right[L_{{\mathrm{\α}}_0}^{r_l-},R_{{\mathrm{\α}}_0}^{r_l+}],[L_{{\mathrm{\α}}_1}^{r_l-},R_{{\mathrm{\α}}_1}^{r_l+}],...,[L_{{\mathrm{\α}}_j}^{r_l-},R_{{\mathrm{\α}}_j}^{r_l+}],...,[L_{{\mathrm{\α}}_{p-1}}^{r_l-},R_{{\mathrm{\α}}_{p-1}}^{r_l+}\left]\right\},$ 表示关于k时刻四旋翼飞行器观测向量z_k中元素r_l,k的观测噪声区间集合，为中的第j个取值为区间的元素，j=0,1,…,p-1，p∈[3,5]，为它的左端点，为它的右端点，且有

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

其中α_j＝j/p，则有

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

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

${\stackrel{\‾}{B}}_R^{r_l}\left(\right[L_{{\mathrm{\α}}_j}^{r_l-},R_{{\mathrm{\α}}_j}^{r_l+}\left]\right)=\left\{\begin{array}{cc}{\mathrm{\α}}_{j+1}& j=0\\ {\mathrm{\α}}_{j+1}-{\mathrm{\α}}_j& j=\mathrm{1,2},...p-2\\ 1-{\mathrm{\α}}_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_{{\mathrm{\α}}_0}^{r_l-},R_{{\mathrm{\α}}_0}^{r_l+}],[L_{{\mathrm{\α}}_1}^{r_l-},R_{{\mathrm{\α}}_1}^{r_l+}],...,[L_{{\mathrm{\α}}_j}^{r_l-},R_{{\mathrm{\α}}_j}^{r_l+}],...,[L_{{\mathrm{\α}}_{p-1}}^{r_l-},R_{{\mathrm{\α}}_{p-1}}^{r_l+}],[{\mathrm{\λ}\·L}_{{\mathrm{\α}}_0}^{r_l-},{\mathrm{\λ}\·R}_{{\mathrm{\α}}_0}^{r_l+}]\right\},\mathrm{\λ}\∈\left[\mathrm{10,100}\right]$ 为中各区间元素的信度赋值组成的集合

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

其中

$B_R^{r_l}\left(\right[L_{{\mathrm{\α}}_j}^{r_l-},R_{{\mathrm{\α}}_j}^{r_l+}\left]\right)=(1-{\mathrm{\ϵ}}_R){\stackrel{\‾}{B}}_R^{r_l}\left(\right[L_{{\mathrm{\α}}_j}^{r_l-},R_{{\mathrm{\α}}_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[{\mathrm{\λ}\·L}_{{\mathrm{\α}}_0}^{r_l-},{\mathrm{\λ}\·R}_{{\mathrm{\α}}_0}^{r_l+}])={\mathrm{\ϵ}}_R---\left(29\right)$

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

为便于理解，这里举例说明，设定p=3，l=1，元素取值分别为 $[L_{{\mathrm{\α}}_0}^{r_l-},R_{{\mathrm{\α}}_0}^{r_l+}]=[-\mathrm{0.0275,0.0275}],[L_{{\mathrm{\α}}_1}^{r_l-},R_{{\mathrm{\α}}_1}^{r_l+}]=[-\mathrm{0.055,0.055}],[L_{{\mathrm{\α}}_2}^{r_l-},R_{{\mathrm{\α}}_2}^{r_l+}]=[-\mathrm{0.0825,0.0825}],{\mathrm{\α}}_j=j/p,$ 则 ${\stackrel{\‾}{B}}_R^{r_l}\left([L_{{\mathrm{\α}}_0}^{r_l-},R_{{\mathrm{\α}}_0}^{r_l+}])=1/3,{\stackrel{\‾}{B}}_R^{r_l}\left(\right[L_{{\mathrm{\α}}_1}^{r_l-},R_{{\mathrm{\α}}_1}^{r_l+}])=2/3-1/3=1/3,{\stackrel{\‾}{B}}_R^{r_1}([L_{{\mathrm{\α}}_2}^{r_1-},R_{{\mathrm{\α}}_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})$ 为

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

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

亦即：

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

$B_{k+1}^{r_l}\left(\right[{\mathrm{\λ}\·L}_{{\mathrm{\α}}_0}^{r_l-}+r_{l,k+1},{\mathrm{\λ}\·R}_{{\mathrm{\α}}_0}^{r_l+}+r_{l,k+1}\left]\right)=B_R^{r_l}\left(\right[\mathrm{\λ}\·L_{{\mathrm{\α}}_0}^{r_l-},{\mathrm{\λ}\·R}_{{\mathrm{\α}}_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_{{\mathrm{\α}}_0}^{r_l-}+{\hat{r}}_{l,k+1},R_{{\mathrm{\α}}_0}^{r_l+}+{\hat{r}}_{l,k+1}]=C_1,[L_{{\mathrm{\α}}_1}^{r_l-}+{\hat{r}}_{l,k+1},R_{{\mathrm{\α}}_1}^{r_l+}+{\hat{r}}_{l,k+1}]=C_2,...,$ $[L_{{\mathrm{\α}}_j}^{r_l-}+{\hat{r}}_{l,k+1},R_{{\mathrm{\α}}_j}^{r_l+}{\hat{r}}_{l,k+1}]=C_{j+1},...,[L_{{\mathrm{\α}}_{p-1}}^{r_l-}+{\hat{r}}_{l,k+1},R_{{\mathrm{\α}}_{p-1}}^{r_l+}+{\hat{r}}_{l,k+1}]=C_p,$ $[{\mathrm{\λ}\·L}_{{\mathrm{\α}}_0}^{r_l-}+{\hat{r}}_{l,k+1},{\mathrm{\λ}\·R}_{{\mathrm{\α}}_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_{\mathrm{\ζ}}^{r_{l,k+1}},R_{\mathrm{\ζ}}^{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_{\mathrm{\ζ}}^{r_{l,k+1}},R_{\mathrm{\ζ}}^{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³个区间，它们组成的区间集合为

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

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

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

其中的为所对应式(7-10)输入的三元区间组中每个区间信度赋值的乘积，由式(33)和(34)可以构成关于q_i,k+1的状态域的初始新证据 ${\stackrel{\‾}{E}}_{k+1}^{q_i}=({\stackrel{\‾}{F}}_{k+1}^{q_i},{\stackrel{\‾}{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_{{\mathrm{\α}}_0}^{q_i-}+{\hat{q}}_{i,k|k},R_{{\mathrm{\α}}_0}^{q_i+}+{\hat{q}}_{i,k|k}]=H_1,[L_{{\mathrm{\α}}_1}^{q_i-}+{\hat{q}}_{i,k|k},R_{{\mathrm{\α}}_1}^{q_i+}+{\hat{q}}_{i,k|k}]=H_2,...,$ $[L_{{\mathrm{\α}}_j}^{q_i-}+{\hat{q}}_{i,k|k},R_{{\mathrm{\α}}_j}^{q_i+}+{\hat{q}}_{i,k|k}]=H_{j+1},...,[L_{{\mathrm{\α}}_{p-1}}^{q_i-}+{\hat{q}}_{i,k|k},R_{{\mathrm{\α}}_{p-1}}^{q_i+}+{\hat{q}}_{i,k|k}]=H_p,$ $[{\mathrm{\λ}\·L}_{{\mathrm{\α}}_0}^{q_i-}+{\hat{q}}_{i,k|k},{\mathrm{\λ}\·R}_{{\mathrm{\α}}_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_{\mathrm{\ζ}}^{q_{i,k+1}},R_{\mathrm{\ζ}}^{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}^{\′}}{\sqrt{{\hat{q}}_{0,k+1|k+1}^{\′2}+{\hat{q}}_{1,k+1|k+1}^{\′2}+{\hat{q}}_{2,k+1|k+1}^{\′2}+{\hat{q}}_{3,k+1|k+1}^{\′2}}}---\left(38\right)$

其中

${\hat{q}}_{i,k+1|k+1}^{\′}=\underset{\mathrm{\ξ}=1}{\overset{m}{\mathrm{\Σ}}}{\hat{B}}_{k+1|k+1}^{q_i}\·\frac{1}{2}(L_{\mathrm{\ξ}}^{q_{i,k+1}}+R_{\mathrm{\ξ}}^{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)得：

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

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

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

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

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_{{\mathrm{\α}}_0}^{q_i-},R_{{\mathrm{\α}}_0}^{q_i+}]=[-\mathrm{0.010,0.010}],[L_{{\mathrm{\α}}_1}^{q_i-},R_{{\mathrm{\α}}_1}^{q_i+}]=[-\mathrm{0.020,0.020}],$ $[L_{{\mathrm{\α}}_2}^{q_i-},R_{{\mathrm{\α}}_2}^{q_i+}]=[-\mathrm{0.030,0.030}],{\mathrm{\α}}_j=j/p,$ 即 ${\stackrel{\‾}{B}}_Q^{q_i}([L_{{\mathrm{\α}}_0}^{q_i-},R_{{\mathrm{\α}}_0}^{q_i+}])=1/3,$ $B_Q^{q_i}\left(\right[L_{{\mathrm{\α}}_1}^{q_i-},R_{{\mathrm{\α}}_1}^{q_i+}\left]\right)=2/3-1/3=1/3,B_Q^{q_i}\left(\right[L_{{\mathrm{\α}}_2}^{q_i-},R_{{\mathrm{\α}}_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所示：

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

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

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

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_{{\mathrm{\α}}_0}^{r_l-},R_{{\mathrm{\α}}_0}^{r_l+}]=[-\mathrm{0.0275,0.0275}],$ $[L_{{\mathrm{\α}}_1}^{r_l-},R_{{\mathrm{\α}}_1}^{r_l+}]=[-\mathrm{0.055,0.055}],[L_{{\mathrm{\α}}_2}^{r_l-},R_{{\mathrm{\α}}_2}^{r_l+}]=[-\mathrm{0.0825,0.0825}],{\mathrm{\α}}_j=j/p,$ 即 ${\stackrel{\‾}{B}}_R^{r_l}\left([L_{{\mathrm{\α}}_0}^{r_l-},R_{{\mathrm{\α}}_0}^{r_l+}])=1/3,{\stackrel{\‾}{B}}_R^{r_l}\left(\right[L_{{\mathrm{\α}}_1}^{r_l-},R_{{\mathrm{\α}}_1}^{r_l+}])=2/3-1/3=1/3,{\stackrel{\‾}{B}}_R^{r_1}([L_{{\mathrm{\α}}_2}^{r_1-},R_{{\mathrm{\α}}_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)，本发明方法给出的估计值用“--*--”表示，GhaliaNassreddine经典算法给出的估计值用“-◇-”表示，陀螺仪测得的观测值用“-+-”表示。图3给出本发明方法、GhaliaNassreddine经典算法和直接观测之间绝对误差的比较。表23给出了三种方法在二十一步的估计中绝对误差的均值，可以看出本发明方法的估计精度要高于其他方法。

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

Claims (1)

1.基于证据融合的四旋翼飞行器姿态估计方法，其特征在于该方法包括以下各步骤：

(1)建立四旋翼飞行器姿态的状态方程与观测方程，具体是：

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

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

式(1)中，k为自然数,表示时刻，x_k＝[q_0,kq_1,kq_2,kq_3,k]^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]$

为状态噪声函数向量，为四元数状态的噪声变化向量，并有

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

(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\left(q_{1,k+1}{q}_{3,k+1}-q_{0,k+1}{q}_{2,k+1}\right))\\ \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₃分别表示横滚角、俯仰角、偏航角，则为观测噪声函数向量，是四旋翼飞行器姿态的观测噪声变化向量，并有

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

四旋翼飞行器姿态的四元数状态向量x_k＝[q_0,kq_1,kq_2,kq_3,k]^T中的各元素与观测向量z_k＝[r_1,kr_2,kr_3,k]^T各元素之间的转换关系函数如式(6)所示

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

其中

$q_{0,k}^{\′}=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}^{\′}=\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}^{\′}=\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}^{\′}=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的证据具体是：

(2-1)构造状态噪声函数向量Q(v)关于四元数状态向量x_k元素q_i,k的初始证据其中表示关于k时刻四旋翼飞行器姿态的四元数状态向量x_k＝[q_0,kq_1,kq_2,kq_3,k]^T中元素q_i,k的状态噪声区间集合，为中的第j个取值为区间的元素，j＝0,1,…,p-1，p∈[3,5]，为它的左端点，为它的右端点，且有

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

其中α_j＝j/p，则有

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

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

${\stackrel{\‾}{B}}_Q^{q_i}=\{{\stackrel{\‾}{B}}_Q^{q_i}(\[L_{{\mathrm{\α}}_0}^{q_i-},R_{{\mathrm{\α}}_0}^{q_i+}\]),{\stackrel{\‾}{B}}_Q^{q_i}(L_{{\mathrm{\α}}_1}^{q_i-},R_{{\mathrm{\α}}_1}^{q_i+}),...,{\stackrel{\‾}{B}}_Q^{q_i}(\[L_{{\mathrm{\α}}_j}^{q_i-},R_{{\mathrm{\α}}_j}^{q_i+}\]),...,{\stackrel{\‾}{B}}_Q^{q_i}(\[L_{{\mathrm{\α}}_{p-1}}^{q_i-},R_{{\mathrm{\α}}_{p-1}}^{q_i+}\])\}$

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

${\stackrel{\‾}{B}}_Q^{q_i}(\[L_{{\mathrm{\α}}_j}^{q_i-},R_{{\mathrm{\α}}_j}^{q_i+}\])=\left\{\begin{array}{cc}{\mathrm{\α}}_{j+1}& j=0\\ {\mathrm{\α}}_{j+1}-{\mathrm{\α}}_j& j=1,2,...p-2\\ 1-{\mathrm{\α}}_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}=\{\[L_{{\mathrm{\α}}_0}^{q_i-},R_{{\mathrm{\α}}_0}^{q_i+}\],\[L_{{\mathrm{\α}}_1}^{q_i-},R_{{\mathrm{\α}}_1}^{q_i+}\],...,\[L_{{\mathrm{\α}}_j}^{q_i-},R_{{\mathrm{\α}}_j}^{q_i+}\],...,\[L_{{\mathrm{\α}}_{p-1}}^{q_i-},R_{{\mathrm{\α}}_{p-1}}^{q_i+}\],\[\mathrm{\λ}\·L_{{\mathrm{\α}}_0}^{q_i-},\mathrm{\λ}\·R_{{\mathrm{\α}}_0}^{q_i+}\]\},\mathrm{\λ}\∈\[10,100\]$

为中各区间元素的信度组成的集合

$B_Q^{q_i}=\{B_Q^{q_i}(\[L_{{\mathrm{\α}}_0}^{q_i-},R_{{\mathrm{\α}}_0}^{q_i+}\]),B_Q^{q_i}(\[L_{{\mathrm{\α}}_1}^{q_i-},R_{{\mathrm{\α}}_1}^{q_i+}\]),...,B_Q^{q_i}(\[L_{{\mathrm{\α}}_j}^{q_i-},R_{{\mathrm{\α}}_j}^{q_i+}\]),...,B_Q^{q_i}(\[L_{{\mathrm{\α}}_{p-1}}^{q_i-},R_{{\mathrm{\α}}_{p-1}}^{q_i+}\]),B_Q^{q_i}(\mathrm{\λ}\·L_{{\mathrm{\α}}_0}^{q_i-},\mathrm{\λ}\·R_{{\mathrm{\α}}_0}^{q_i+})\}$

其中

$\begin{array}{ccc}{B}_Q^{q_i}(\[L_{{\mathrm{\α}}_j}^{q_i-},R_{{\mathrm{\α}}_j}^{q_i+}\])=(1-{\mathrm{\ϵ}}_Q){\stackrel{\‾}{B}}_Q^{q_i}(\[L_{{\mathrm{\α}}_j}^{q_i-},R_{{\mathrm{\α}}_j}^{q_i+}\])& j=0,1,...,p-1& i=0,1,2,3\end{array}---\left(13\right)$

$B_Q^{q_i}(\[\mathrm{\λ}\·L_{{\mathrm{\α}}_0}^{q_i-},\mathrm{\λ}\·R_{{\mathrm{\α}}_0}^{q_i+}\])={\mathrm{\ϵ}}_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,kr_2,kr_3,k分别带入式(7)-(10)得到

$q_{0,0|0}^{\′}=\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}^{\′}=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}^{\′}=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}^{\′}=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}^{\′}}{\sqrt{q_{0,0|0}^{\′2}+q_{1,0|0}^{\′2}+q_{2,0|0}^{\′2}+q_{3,0|0}^{\′2}}},i=0,1,2,3$

由此得到状态估计向量则状态估计证据中的

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

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

亦即

$\begin{array}{ccc}{B}_{0|0}^{q_i}(\[L_{{\mathrm{\α}}_j}^{q_i-}+{\hat{q}}_{i,0|0},R_{{\mathrm{\α}}_j}^{q_i+}+{\hat{q}}_{i,0|0}\])=B_Q^{q_i}(\[L_{{\mathrm{\α}}_j}^{q_i-},R_{{\mathrm{\α}}_j}^{q_i+}\])& j=0,1,...,p-1& i=0,1,2,3\end{array}$

$B_{0|0}^{q_i}(\[\mathrm{\λ}\·L_{{\mathrm{\α}}_0}^{q_i-}+{\hat{q}}_{i,0|0},\mathrm{\λ}\·R_{{\mathrm{\α}}_0}^{q_i+}+{\hat{q}}_{i,0|0}\])=B_Q^{q_i}(\[\mathrm{\λ}\·L_{{\mathrm{\α}}_0}^{q_i-},\mathrm{\λ}\·R_{{\mathrm{\α}}_0}^{q_i+}\])$

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

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

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

亦即

$\begin{array}{ccc}{B}_{k|k}^{q_i}(\[L_{{\mathrm{\α}}_j}^{q_i-}+{\hat{q}}_{i,k|k},R_{{\mathrm{\α}}_j}^{q_i+}+{\hat{q}}_{i,k|k}\])=B_Q^{q_i}(\[L_{{\mathrm{\α}}_j}^{q_i-},R_{{\mathrm{\α}}_j}^{q_i+}\])& j=0,1,...,p-1& i=0,1,2,3\end{array}$

$B_{k|k}^{q_i}(\[\mathrm{\λ}\·L_{{\mathrm{\α}}_0}^{q_i-}+{\hat{q}}_{i,k|k},\mathrm{\λ}\·R_{{\mathrm{\α}}_0}^{q_i+}+{\hat{q}}_{i,k|k}\])=B_Q^{q_i}(\[\mathrm{\λ}\·L_{{\mathrm{\α}}_0}^{q_i-},\mathrm{\λ}\·R_{{\mathrm{\α}}_0}^{q_i+}\])$

(3-2)通过状态转移矩阵A获取向量x_k的元素q_i,k的预测证据其中

$\begin{array}{c}{F}_{k+1|k}^{q_i}=\{\[A_{i+1,i+1}(L_{{\mathrm{\α}}_0}^{q_i-}+{\hat{q}}_{i,k|k}),A_{i+1,i+1}(R_{{\mathrm{\α}}_0}^{q_i+}+{\hat{q}}_{i,k|k})\],\[A_{i+1,i+1}(L_{{\mathrm{\α}}_1}^{q_i-}+{\hat{q}}_{i,k|k}),A_{i+1,i+1}(R_{{\mathrm{\α}}_1}^{q_i+}+{\hat{q}}_{i,k|k})\],...,\\ \[A_{i+1,i+1}(L_{{\mathrm{\α}}_j}^{q_i-}+{\hat{q}}_{i,k|k}),A_{i+1,i+1}(R_{{\mathrm{\α}}_j}^{q_i+}+{\hat{q}}_{i,k|k})\],...,\[A_{i+1,i+1}(L_{{\mathrm{\α}}_{p-1}}^{q_i-}+{\hat{q}}_{i,k|k}),\\ A_{i+1,i+1}(R_{{\mathrm{\α}}_{p-1}}^{q_i+}+{\hat{q}}_{i,k|k})\],\[A_{i+1,i+1}(\mathrm{\λ}\·L_{{\mathrm{\α}}_0}^{q_i-}+{\hat{q}}_{i,k|k}),A_{i+1,i+1}(\mathrm{\λ}\·R_{{\mathrm{\α}}_0}^{q_i+}+{\hat{q}}_{i,k|k})\]\}\end{array}---\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}(\[A_{i+1,i+1}(L_{{\mathrm{\α}}_j}^{q_i-}+{\hat{q}}_{i,k|k}),A_{i+1,i+1}(R_{{\mathrm{\α}}_j}^{q_i+}+{\hat{q}}_{i,k|k})\])=B_{k|k}^{q_i}(\[L_{{\mathrm{\α}}_j}^{q_i-}+{\hat{q}}_{i,k|k},R_{{\mathrm{\α}}_j}^{q_i+}+{\hat{q}}_{i,k|k}\])---\left(20\right)$

$B_{k|k}^{q_i}(\[A_{i+1,i+1}(\mathrm{\λ}\·L_{{\mathrm{\α}}_0}^{q_i-}+{\hat{q}}_{i,k|k}),A_{i+1,i+1}(\mathrm{\λ}\·R_{{\mathrm{\α}}_0}^{q_i+}+{\hat{q}}_{i,k|k})\])=B_{k|k}^{q_i}(\[\mathrm{\λ}\·L_{{\mathrm{\α}}_0}^{q_i-}+{\hat{q}}_{i,k|k},\mathrm{\λ}\·R_{{\mathrm{\α}}_0}^{q_i+}+{\hat{q}}_{i,k|k}\])---\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)⁴个区间，它们组成的区间集合为

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

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

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

其中的为所对应式(4)输入的四元区间组中每个区间信度赋值的乘积，由式(22)和(23)可以构成关于r_l,k+1的初始观测预测证据

(4-2)将步骤(4-1)所得的进行简化后得到元素r_l,k+1的观测预测证据其中

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

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

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

(5)利用Dempster组合规则求出k+1时刻观测域的融合证据具体是：

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

(a)构造观测噪声函数向量R(w)关于观测向量z_k+1元素r_l,k+1的初始证据其中表示关于k时刻四旋翼飞行器姿态观测向量z_k中元素r_l,k的观测噪声区间集合，为中的第j个取值为区间的元素，j＝0,1,…,p-1，p∈[3,5]，为它的左端点，为它的右端点，且有

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

其中α_j＝j/p，则有

$\[L_{{\mathrm{\α}}_0}^{r_l-},R_{{\mathrm{\α}}_0}^{r_l+}\]\⊃\[L_{{\mathrm{\α}}_1}^{r_l-},R_{{\mathrm{\α}}_1}^{r_l+}\]\⊃...\[L_{{\mathrm{\α}}_j}^{r_l-},R_{{\mathrm{\α}}_j}^{r_l+}\]\⊃...\⊃\[L_{{\mathrm{\α}}_{p-2}}^{r_l-},R_{{\mathrm{\α}}_{p-2}}^{r_l+}\]\⊃\[L_{{\mathrm{\α}}_{p-1}}^{r_l-},R_{{\mathrm{\α}}_{p-1}}^{r_l+}\]$

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

${\stackrel{\‾}{B}}_R^{r_l}=\{{\stackrel{\‾}{B}}_R^{r_l}(\[L_{{\mathrm{\α}}_0}^{r_l-},R_{{\mathrm{\α}}_0}^{r_l+}\]),{\stackrel{\‾}{B}}_R^{r_l}(L_{{\mathrm{\α}}_1}^{r_l-},R_{{\mathrm{\α}}_1}^{r_l+}),...,{\stackrel{\‾}{B}}_R^{r_l}(\[L_{{\mathrm{\α}}_j}^{r_l-},R_{{\mathrm{\α}}_j}^{r_l+}\]),...,{\stackrel{\‾}{B}}_R^{r_l}(\[L_{{\mathrm{\α}}_{p-1}}^{r_l-},R_{{\mathrm{\α}}_{p-1}}^{r_l+}\])\}$

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

${\stackrel{\‾}{B}}_R^{r_l}(\[L_{{\mathrm{\α}}_j}^{r_l-},R_{{\mathrm{\α}}_j}^{r_l+}\])=\left\{\begin{array}{cc}{\mathrm{\α}}_{j+1}& j=0\\ {\mathrm{\α}}_{j+1}-{\mathrm{\α}}_j& j=1,2,...p-2\\ 1-{\mathrm{\α}}_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}=\{\[L_{{\mathrm{\α}}_0}^{r_l-},R_{{\mathrm{\α}}_0}^{r_l+}\],\[L_{{\mathrm{\α}}_1}^{r_l-},R_{{\mathrm{\α}}_1}^{r_l+}\],...,\[L_{{\mathrm{\α}}_j}^{r_l-},R_{{\mathrm{\α}}_j}^{r_l+}\],...,\[L_{{\mathrm{\α}}_{p-1}}^{r_l-},R_{{\mathrm{\α}}_{p-1}}^{r_l+}\],\[\mathrm{\λ}\·L_{{\mathrm{\α}}_0}^{r_l-},\mathrm{\λ}\·R_{{\mathrm{\α}}_0}^{r_l+}\]\},\mathrm{\λ}\∈\[10,100\]$

为中各区间元素的信度赋值组成的集合

$B_R^{r_l}=\{B_R^{r_l}(\[L_{{\mathrm{\α}}_0}^{r_l-},R_{{\mathrm{\α}}_0}^{r_l+}\]),B_R^{r_l}(\[L_{{\mathrm{\α}}_1}^{r_l-},R_{{\mathrm{\α}}_1}^{r_l+}\]),...,B_R^{r_l}(\[L_{{\mathrm{\α}}_j}^{r_l-},R_{{\mathrm{\α}}_j}^{r_l+}\]),...,B_R^{r_l}(\[L_{{\mathrm{\α}}_{p-1}}^{r_l-},R_{{\mathrm{\α}}_{p-1}}^{r_l+}\]),B_R^{r_l}(\mathrm{\λ}\·L_{{\mathrm{\α}}_0}^{r_r-},\mathrm{\λ}\·R_{{\mathrm{\α}}_0}^{r_l+})\}$

其中

$\begin{array}{ccc}{B}_R^{r_l}(\[L_{{\mathrm{\α}}_j}^{r_l-},R_{{\mathrm{\α}}_j}^{r_l+}\])=(1-{\mathrm{\ϵ}}_R){\stackrel{\‾}{B}}_R^{r_l}(\[L_{{\mathrm{\α}}_j}^{r_l-},R_{{\mathrm{\α}}_j}^{r_l+}\])& j=0,1,...,p-1& i=0,1,2,3\end{array}---\left(28\right)$

$B_R^{r_l}(\[\mathrm{\λ}\·L_{{\mathrm{\α}}_0}^{r_l-},\mathrm{\λ}\·R_{{\mathrm{\α}}_0}^{r_l+}\])={\mathrm{\ϵ}}_R---\left(29\right)$

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

(c)当从陀螺仪观测到向量z_k+1＝[r_1,k+1r_2,k+1r_3,k+1]^T，则构建元素r_l,k+1的观测证据为

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

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

亦即：

$\begin{array}{ccc}{B}_{k+1}^{r_l}(\[L_{{\mathrm{\α}}_j}^{r_l-}+r_{l,k+1},R_{{\mathrm{\α}}_j}^{r_l+}+r_{l,k+1}\])=B_R^{r_l}(\[L_{{\mathrm{\α}}_j}^{r_l-},R_{{\mathrm{\α}}_j}^{r_l+}\])& j=0,1,...,p-1& l=1,2,3\end{array}$

$B_{k+1}^{r_l}(\[\mathrm{\λ}\·L_{{\mathrm{\α}}_0}^{r_l-}+r_{l,k+1},\mathrm{\λ}\·R_{{\mathrm{\α}}_0}^{r_l+}+r_{l,k+1}\])=B_R^{r_l}(\[\mathrm{\λ}\·L_{{\mathrm{\α}}_0}^{r_l-},\mathrm{\λ}\·R_{{\mathrm{\α}}_0}^{r_l+}\])$

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

令其信度赋值为令 其信度赋值为

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

其中表示通过A_g∩C_h共计生成了n个不同的区间则观测域的融合证据为

中对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³个区间，它们组成的区间集合为

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

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

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

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

(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)中的为剩余各区间取并后得到的区间，且这些区间的信度相加后构成

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

将步骤(6)中得到的关于向量x_k+1的元素q_i,k+1的状态域的新证据和步骤(3)中得到的关于向量x_k的元素q_i,k的预测证据利用Dempster组合规则进行证据融合，得到k+1时刻状态估计的融合证据

令其信度赋值为令 其信度赋值为

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

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

${\hat{F}}_{k+1|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}}\],...,\[L_{\mathrm{\ξ}}^{q_{i,k+1}},R_{\mathrm{\ξ}}^{q_{i,k+1}}\],...,\[L_m^{q_{i,k+1}},R_m^{q_{i,k+1}}\]\}$

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

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

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

其中，

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

然后将获得的k+1时刻状态估计向量带入步骤(3)进行下一时刻算法迭代，便可递推得出每一时刻的状态估计。