[发明专利]一种基于卡尔曼滤波的角加速度估计方法

摘要

本发明公开了一种基于卡尔曼滤波的角加速度估计方法，用于解决增量动态逆控制(INDI)利用角加速度反馈进行解算，而一般飞控硬件采用惯性测量单元测量角速度，没有角加速度传感器直接测量角加速度的问题。直接利用角速度差分得到的角加速度噪声过大，不能用于控制律的解算，因此需要利用传感器的信号估计出角加速度信号。常规的方法是对角速度信号直接进行差分之后再低通滤波，但这样会带来额外的时间延迟。本发明提供的用卡尔曼滤波进行角加速度估计可以去除高频噪声并保证较小的延迟。该算法因涉及了实际工程背景，且简单方便易于实施，具有较高的工程应用价值。

主权项

1.一种基于卡尔曼滤波的角加速度估计方法，用于增量动态逆控制中对角加速度的解算，其特征在于，包括如下几个步骤：S1：建立卡尔曼滤波模型，状态量为分别为角速度、角加速度和角加速度的导数，飞行器的状态方程和观测方程如下：z_k+1＝[1 0 0]x_k+1+R,H＝[1 0 0]ΔT为采样周期，z为观测量，观测量为俯仰角速度，来源为IMU的角速率。Q和R分别为过程噪声和观测噪声；S2：滤波估计过程：一步状态更新，根据运动学方程求解出下一采样时刻的状态量S3:根据上一采样时刻状态误差协方差进行进一步更新，得到S4：根据和H，求解卡尔曼增益；S5：进行姿态角运动状态量和协方差校正。