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

摘要

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

主权项

基于证据融合的四旋翼飞行器姿态估计方法，其特征在于该方法包括以下各步骤：(1)建立四旋翼飞行器姿态的状态方程与观测方程，具体是：(1‑1)建立四旋翼飞行器姿态的状态方程如式(1)所示：x_k+1＝Ax_k+Q(v)       (1)式(1)中，k为自然数,表示时刻，x_k＝[q_0,k q_1,k q_2,k q_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,k q_1,k q_2,k q_3,k]^T中的各元素与观测向量z_k＝[r_1,k r_2,k r_3,k]^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的证据具体是：(2‑1)构造状态噪声函数向量Q(v)关于四元数状态向量x_k元素q_i,k的初始证据其中表示关于k时刻四旋翼飞行器姿态的四元数状态向量x_k＝[q_0,k q_1,k q_2,k q_3,k]^T中元素q_i,k的状态噪声区间集合，为中的第j个取值为区间的元素，j＝0,1,…,p‑1，p∈[3,5]，为它的左端点，为它的右端点，且有$\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}---\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}=\{{\bar{B}}_Q^{q_i}\left(\right[L_{{\alpha }_0}^{q_i-},R_{{\alpha }_0}^{q_i+}\left]\right),{\bar{B}}_Q^{q_i}(L_{{\alpha }_1}^{q_i-},R_{{\alpha }_1}^{q_i+}),...,{\bar{B}}_Q^{q_i}\left(\right[L_{{\alpha }_j}^{q_i-},R_{{\alpha }_j}^{q_i+}\left]\right),...,{\bar{B}}_Q^{q_i}\left(\right[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=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\{\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+}],[\lambda ·L_{{\alpha }_0}^{q_i-},\lambda ·R_{{\alpha }_0}^{q_i+}\left]\right\},\lambda \in [10,100]$为中各区间元素的信度组成的集合$B_Q^{q_i}=\{B_Q^{q_i}\left(\right[L_{{\alpha }_0}^{q_i-},R_{{\alpha }_0}^{q_i+}\left]\right),B_Q^{q_i}\left(\right[L_{{\alpha }_1}^{q_i-},R_{{\alpha }_1}^{q_i+}\left]\right),...,B_Q^{q_i}\left(\right[L_{{\alpha }_j}^{q_i-},R_{{\alpha }_j}^{q_i+}\left]\right),...,B_Q^{q_i}\left(\right[L_{{\alpha }_{p-1}}^{q_i-},R_{{\alpha }_{p-1}}^{q_i+}\left]\right),B_Q^{q_i}(\lambda ·L_{{\alpha }_0}^{q_i-},\lambda ·R_{{\alpha }_0}^{q_i+})\}$其中$\begin{array}{ccc}{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=0,1,...,p-1& i=0,1,2,3\end{array}---\left(13\right)$$B_Q^{q_i}\left(\right[\lambda ·L_{{\alpha }_0}^{q_i-},\lambda ·R_{{\alpha }_0}^{q_i+}\left]\right)={ϵ}_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_{0,0|0}^{\prime 2}+q_{1,0|0}^{\prime 2}+q_{2,0|0}^{\prime 2}+q_{3,0|0}^{\prime 2}}},i=0,1,2,3$由此得到状态估计向量则状态估计证据中的$\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}---\left(15\right)$$B_{0|0}^{q_i}=B_Q^{q_i}---\left(16\right)$亦即$\begin{array}{ccc}{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=0,1,...,p-1& i=0,1,2,3\end{array}$$B_{0|0}^{q_i}\left(\right[\lambda ·L_{{\alpha }_0}^{q_i-}+{\hat{q}}_{i,0|0},\lambda ·R_{{\alpha }_0}^{q_i+}+{\hat{q}}_{i,0|0}\left]\right)=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的状态估计证据为$\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}---\left(17\right)$$B_{k|k}^{q_i}=B_Q^{q_i}---\left(18\right)$亦即$\begin{array}{ccc}{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=0,1,...,p-1& i=0,1,2,3\end{array}$$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)=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的预测证据其中$\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}---\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},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)的输入量，利用MATLAB 2010a软件中的工具箱intlab，分别得到关于r_l,k+1的(p+1)⁴个区间，它们组成的区间集合为${\bar{F}}_{k+1|k}^{r_l}=\left\{\right[{\bar{L}}_1^{r_{l,k+1|k}},{\bar{R}}_1^{r_{l,k+1|k}}],[{\bar{L}}_2^{r_{l,k+1|k}},{\bar{R}}_2^{r_{l,k+1|k}}],...,[{\bar{L}}_{\tau }^{r_{l,k+1|k}},{\bar{R}}_{\tau }^{r_{l,k+1|k}}],...,[{\bar{L}}_{{(p+1)}^4}^{r_{l,k+1|k}},{\bar{R}}_{{(p+1)}^4}^{r_{l,k+1|k}}\left]\right\},\tau =1,2,3,...,{(p+1)}^4---\left(22\right)$并得到式(22)中每个区间的信度赋值组成的信度集合为${\bar{B}}_{k+1|k}^{r_l}=\{{\bar{B}}_{k+1|k}^{r_l}\left(\right[{\bar{L}}_1^{r_{l,k}},{\bar{R}}_1^{r_{l,k}}\left]\right),{\bar{B}}_{k+1|k}^{r_l}\left(\right[{\bar{L}}_2^{r_{l,k+1|k}},{\bar{R}}_2^{r_{l,k+1|k}}\left]\right),...,{\bar{B}}_{k+1|k}^{r_l}\left(\right[{\bar{L}}_{\tau }^{r_{l,k+1|k}},{\bar{R}}_{\tau }^{r_{l,k+1|k}}\left]\right),...,{\bar{B}}_{k+1|k}^{r_l}\left(\right[{\bar{L}}_{{(p+1)}^4}^{r_{l,k+1|k}},{\bar{R}}_{{(p+1)}^4}^{r_{l,k+1|k}}\left]\right)\}---\left(23\right)$其中的为所对应式(4)输入的四元区间组中每个区间信度赋值的乘积，由式(22)和(23)可以构成关于r_l,k+1的初始观测预测证据(4‑2)将步骤(4‑1)所得的进行简化后得到元素r_l,k+1的观测预测证据其中$F_{k+1|k}^{r_l}=\left\{\right[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]\right\}---\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时刻观测域的融合证据具体是：(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}_{{\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}---\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}=\{{\bar{B}}_R^{r_l}\left(\right[L_{{\alpha }_0}^{r_l-},R_{{\alpha }_0}^{r_l+}\left]\right),{\bar{B}}_R^{r_l}(L_{{\alpha }_1}^{r_l-},R_{{\alpha }_1}^{r_l+}),...,{\bar{B}}_R^{r_l}\left(\right[L_{{\alpha }_j}^{r_l-},R_{{\alpha }_j}^{r_l+}\left]\right),...,{\bar{B}}_R^{r_l}\left(\right[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=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\{\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+}],[\lambda ·L_{{\alpha }_0}^{r_l-},\lambda ·R_{{\alpha }_0}^{r_l+}\left]\right\},\lambda \in [10,100]$为中各区间元素的信度赋值组成的集合$B_R^{r_l}=\{B_R^{r_l}\left(\right[L_{{\alpha }_0}^{r_l-},R_{{\alpha }_0}^{r_l+}\left]\right),B_R^{r_l}\left(\right[L_{{\alpha }_1}^{r_l-},R_{{\alpha }_1}^{r_l+}\left]\right),...,B_R^{r_l}\left(\right[L_{{\alpha }_j}^{r_l-},R_{{\alpha }_j}^{r_l+}\left]\right),...,B_R^{r_l}\left(\right[L_{{\alpha }_{p-1}}^{r_l-},R_{{\alpha }_{p-1}}^{r_l+}\left]\right),B_R^{r_l}(\lambda ·L_{{\alpha }_0}^{r_r-},\lambda ·R_{{\alpha }_0}^{r_l+})\}$其中$\begin{array}{ccc}{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=0,1,...,p-1& i=0,1,2,3\end{array}---\left(28\right)$$B_R^{r_l}\left(\right[\lambda ·L_{{\alpha }_0}^{r_l-},\lambda ·R_{{\alpha }_0}^{r_l+}\left]\right)={ϵ}_R---\left(29\right)$这里折扣率ε_R∈[0.03,0.08]；(c)当从陀螺仪观测到向量z_k+1＝[r_1,k+1 r_2,k+1 r_3,k+1]^T，则构建元素r_l,k+1的观测证据为$\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}---\left(30\right)$$B_{k+1}^{r_l}=B_R^{r_l}---\left(31\right)$亦即：$\begin{array}{ccc}{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=0,1,...,p-1& l=1,2,3\end{array}$$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组合规则进行证据融合，得到观测域的融合证据令其信度赋值为令其信度赋值为利用Dempster组合规则将和组合后得到其中表示通过A_g∩C_h共计生成了n个不同的区间则观测域的融合证据为中对n个不同区间的信度赋值，由式(32)得到；(6)通过观测方程的逆运算获得k+1时刻关于向量x_k+1的元素q_i,k+1的状态域的新证据具体是：(6‑1)由步骤(5)得到的观测域的融合证据之后，分别抽取中的一个元素进行排列组合，共计产生n³个三元区间组，将这些三元区间组分别作为式(7)‑(10)的输入量，利用MATLAB 2010a软件中的工具箱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 =1,2,3,...,n^3---\left(33\right)$并可得到式(33)中每个区间的信度赋值组成的信度集合为${\bar{B}}_{k+1}^{q_i}=\{{\bar{B}}_{k+1}^{q_i}\left(\right[{\bar{L}}_1^{q_{i,k+1}},{\bar{R}}_1^{q_{i,k+1}}\left]\right),{\bar{B}}_{k+1}^{q_i}\left(\right[{\bar{L}}_2^{q_{i,k+1}},{\bar{R}}_2^{q_{i,k+1}}\left]\right),...,{\bar{B}}_{k+1}^{q_i}\left(\right[{\bar{L}}_{\zeta }^{q_{i,k+1}},{\bar{R}}_{\zeta }^{q_{i,k+1}}\left]\right),...,{\bar{B}}_{k+1}^{q_i}\left(\right[{\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的状态域的初始新证据(6‑2)将步骤(6‑1)所得的进行简化后得到关于元素q_i,k+1的状态域的初始新证据其中${\hat{F}}_{k+1}^{q_i}=\left\{\right[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]\right\}---\left(35\right)$${\hat{B}}_{k+1}^{q_i}=\{{\hat{B}}_{k+1}^{q_i}\left(\right[L_1^{q_{i,k+1}},R_1^{q_{i,k+1}}\left]\right),{\hat{B}}_{k+1}^{q_i}\left(\right[L_2^{q_{i,k+1}},R_2^{q_{i,k+1}}\left]\right)\}---\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}=\left\{\right[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_{\xi }^{q_{i,k+1}},R_{\xi }^{q_{i,k+1}}],...,[L_m^{q_{i,k+1}},R_m^{q_{i,k+1}}\left]\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+1k+1}^{\prime 2}+{\hat{q}}_{1,k+1k+1}^{\prime 2}+{\hat{q}}_{2,k+1k+1}^{\prime 2}+{\hat{q}}_{3,k+1k+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时刻状态估计向量带入步骤(3)进行下一时刻算法迭代，便可递推得出每一时刻的状态估计。