1.一种噪声自适应强跟踪扩展卡尔曼滤波器四旋翼姿态解算方法,其特征在于,包括如下步骤:S1:通过状态转移矩阵构建系统状态方程和观测方程,初始化系统噪声Q和量测噪声初值S2:将本时刻二次估计值 与状态转移矩阵 相乘得到下一时刻的一次预测状态
S3:计算新息,更新强跟踪滤波器计算过程的变量Vk+1、Mk+1、Nk+1,然后计算次优渐消因子λk+1,并将次优渐消因子拆分成两个开平方的形式;
S4:将步骤S3拆分形式的次优渐消因子分别乘在在一步预测协方差矩阵Pk+1|k的两侧,得到改进的一步预测误差协方差矩阵Pk+1|k;
S5:对下一时刻的噪声方差 及噪声均值 重新估计,并将估计值引入EKF公式中;
S6:计算卡尔曼增益矩阵Kk+1、状态 的二次估计和误差协方差Pk+1|k+1的二次估计,判断是否循环结束,若循环结束转至步骤S7;若未结束则循环转回至步骤S2;
S7:输出实验结果。
2.根据权利要求1所述的一种噪声自适应强跟踪扩展卡尔曼滤波器四旋翼姿态解算方法,其特征在于:所述步骤S1采用旋转矩阵 进行构建状态方程和观测方程,具体过程如下:式(15)中,θ、 φ分别为俯仰角、偏航角、横滚角;
式(16) 表示导航系相对于机体系的角速度在机体系的投影, 表示 的叉乘矩阵,从中提取出第三列可以得到:
其中,x为
bg,k+1=bg,k+wg,k (18)式(18)中,bg,k、wg,k分别表示陀螺仪k时刻的漂移和噪声,bg,k+1表示k+1时刻的陀螺仪漂移;
定义系统状态模型:
为了在实际计算机上运行,将式(19)使用一阶差分,使函数离散化,得到系统状态方程:式(20)中Ts表示两次采样间隔时间,程序中表示积分时间;xk为k刻的状态值,ωb,k为陀螺仪k时刻测量值;
对状态模型式(20)使用泰勒级数展开,得到EKF所需要的信息,则系统状态:状态转移矩阵和噪声驱动矩阵分别为:
观测值Zk+1为加速度计观测值可表示为:
系统的观测方程可表示:
观测矩阵为:
Hk+1=[03×3 ‑gI3] (25)。
3.根据权利要求1所述的一种噪声自适应强跟踪扩展卡尔曼滤波器四旋翼姿态解算方法,其特征在于:所述步骤S3的具体过程为将不同时刻新息序列保持正交,并构建强跟踪滤波器,然后计算次优渐消因子λk+1,并将次优渐消因子拆分成两个开平方的形式,将次优渐消因子的拆分式引入扩展卡尔曼滤波器中。
4.根据权利要求1所述的一种噪声自适应强跟踪扩展卡尔曼滤波器四旋翼姿态解算方法,其特征在于:所述步骤S5的具体过程为引入噪声自适应机制,以固定初值的噪声,实时动态调整噪声,来适应系统需要具,体公式步骤如下:Pk+1|k+1=(I‑Kk+1Hk+1)Pk+1|k (14)上式中, 分别为估计噪声方差、噪声均值,Sk+1为新息,R0、r0分别为噪声方差和均值的初值随着k的增加,保证系统有一定的噪声初值,在迭代过程中不至于令卡尔曼增益Kk+1过大或过小,对系统失去调节作用;Vk+1、Mk+1、Nk+1为计算次优渐消因子的中间量,λk+1为次优渐消因子, Pk+1|k为一步预测协方差矩阵。