[发明专利]一种基于扩展卡尔曼滤波的无人机角加速度估计方法及装置

权利要求书

1.一种基于扩展卡尔曼滤波的无人机角加速度估计方法，其特征在于，包括如下步骤：

S1初始化，用以确定状态矢量和观测矢量；

S2预测过程，根据上述状态矢量和观测矢量转化得到非线性动力学模型；

基于非线性动力学模型进行初始估计，向前推算得到第一步的k+1先验状态估计：

向前推算出误差协方差：

其中，测量噪声矩阵Q_k保持变量σ²作为状态量对角项，用以作为滤波器的变量，

S3校正过程，建立观测方程，计算卡尔曼增益：

由观测变量z_k更新后验估计：

更新误差协方差：

继续步骤S2，进行迭代，

观测矩阵H_k和噪声矩阵R_k根据某个新测量值获得而改变，当只能够获得陀螺仪测量值时观测变量：

当只能获得陀螺仪和加速度计时观测变量：

当只能获得陀螺仪和磁强计时观测变量：

2.根据权利要求1所述的无人机角加速度估计方法，其特征在于，还包括：对于第一次迭代的更新后验估计，初始值的选择如下：

第一次误差协方差矩阵初始值为P_k,init＝100I。

3.根据权利要求1所述的无人机角加速度估计方法，其特征在于，所述非线性动力学模型具体为：

4.根据权利要求1所述的无人机角加速度估计方法，其特征在于，进一步还包括将陀螺仪，磁罗盘和加速度计原始值进行滤波，

采用3*3的旋转矩阵表示方位，

Z轴方向上的基准矢量为：

Y轴方向上的基准矢量为：

X轴轴方向基准矢量为：

5.根据权利要求1所述的无人机角加速度估计方法，其特征在于，在所述S3校正过程中，所述观测方程具体为：

6.根据权利要求1所述的无人机角加速度估计方法，其特征在于，确定状态矢量和观测矢量如下:

其中，Bω_IB＝[ω_x,ω_y,ω_z]^T为四旋翼角速率，为四旋翼角加速度的估计值，Br_g＝[Br_g,x,Br_g,y,Br_g,z]^T为地球重力场矢量，Br_m＝[Br_m,x,Br_m,y,Br_m,z]^T为地球磁场矢量，在实际中可以测量的量为：陀螺仪获取加速度计获取磁强计获取

7.基于扩展卡尔曼滤波的无人机角加速度估计装置，其特征在于，采用如权利要求1所述的基于扩展卡尔曼滤波的无人机角加速度估计方法，所述装置包括：

初始化单元被配置为，进行初始化，用以确定状态矢量和观测矢量；

预测单元被配置为，完成预测过程，根据上述状态矢量和观测矢量转化得到非线性动力学模型，基于非线性动力学模型进行初始估计，向前推算得到第一步的k+1先验状态估计：

向前推算出误差协方差：

其中，测量噪声矩阵Q_k保持变量σ²作为状态量对角项，用以作为滤波器的变量，

校正单元被配置为，建立观测方程，计算卡尔曼增益：

由观测变量z_k更新后验估计：

更新误差协方差：

继续步骤S2，进行迭代。