[发明专利]基于扩维模型的容积卡尔曼滤波提取捷联导引头制导信息的方法

权利要求书

1.一种基于扩维模型的容积卡尔曼滤波提取捷联导引头制导信息的方法，其特征在于，它包括以下步骤：

步骤一：通过捷联导引头量测获得弹体系下的体视线角；

步骤二：建立弹目相对运动方程，对弹目相对运动方程两次求导获得惯性视线角速度的二阶导数与弹目相对加速度的关系方程；

步骤三：利用状态变量建立状态方程；

步骤四：根据弹体系到地面系的坐标转换，推导由惯性视线角结合弹体姿态信息，得到弹体系下体视线角的转换方程；并推导弹目距离变化率与弹载GPS输出的高度变化率的关系方程；将以上两方程结合建立观测方程；

步骤五：应用五阶容积卡尔曼滤波提取捷联导引头制导信息；

步骤二中弹目相对运动方程的获得过程为：

根据导弹和目标的空间几何关系和运动学关系建立以下关系式：

式中r为惯性系下导弹与目标的相对位置矢量，r_T为目标在惯性系下的位置矢量，r_M为导弹在惯性系下的位置矢量，V为导弹与目标在惯性系下的相对速度矢量，V_T为目标在惯性系下的速度矢量，V_M为导弹在惯性系下的速度矢量，a为惯性系下导弹与目标的相对加速度矢量，a_T为目标在惯性系下的加速度矢量，a_M为导弹在惯性系下的加速度矢量；

视线坐标系相对惯性坐标系的旋转角速度ω_s为：

q_γ为惯性视线高低角，q_λ为惯性视线方位角，i_s为视线坐标系的x轴单位向量，j_s为视线坐标系的y轴单位向量，k_s为视线坐标系的z轴单位向量；

在视线坐标系中，

同理：

则：

基于弹目相对运动关系，得到弹目相对运动方程：

式中a_xs为导弹相对目标的加速度在视线坐标系的Ox_s轴上的分量，a_ys为导弹相对目标的加速度在视线坐标系的Oy_s轴上的分量，a_zs为导弹相对目标的加速度在视线坐标系的Oz_s轴上的分量；

导弹与目标的相对位置矢量r的估计方法如下：

式中，y_m为弹载GPS输出的高度，y_t为目标点高程；

弹目距离变化率由式(2)两边对t求导获得：

其中，弹载GPS输出的高度变化率，为目标高度变化率，对地面目标近似为零；

步骤三中，利用状态变量建立状态方程的具体方法为：

取状态量为联立式(1)～(3)，建立扩维状态方程：

式中x₁～x₆为状态量x中第1～6个元素。

2.根据权利要求1所述的基于扩维模型的容积卡尔曼滤波提取捷联导引头制导信息的方法，其特征在于，

步骤四中建立观测方程的具体方法为：

根据弹目距离变化率与弹载GPS输出的高度变化率的关系，取观测变量为导引头量测的体高低视线角q_α、导引头量测的体方位视线角q_β和弹载GPS输出的高度变化率即建立观测方程如下：

式中z₁～z₃为观测变量z中第1～3个元素；

其中，上式中的R_bc为地面系到弹体系的转换矩阵L(γ,θ,ψ)中第b行第c列的元素，θ为俯仰角，ψ为偏航角，γ为滚转角，v₁为体高低视线角的噪声信号，v₂为体方位视线角的噪声信号，v₃为弹载GPS高度变化率的测量噪声信号。

3.根据权利要求2所述的基于扩维模型的容积卡尔曼滤波提取捷联导引头制导信息的方法，其特征在于，步骤五中应用五阶容积卡尔曼滤波提取捷联导引头制导信息的具体方法为：

(一)设定初始参数：

设定滤波初始时刻系统状态值x₀，初始时刻状态误差协方差阵为P₀，系统噪声协方差阵Q，量测噪声协方差阵R；

(二)时间更新：

1)假设k-1时刻状态量真实值为x_k-1，估计值为状态误差协方差阵为P_k-1，其后验分布概率已知，对P_k-1进行Cholesky分解：

2)计算容积点：

其中，i＝1,2,…,n，且j＝1,2,…,n(n-1)/2，w₀为第一类容积点的权值，w₁为第二类容积点的权值，w₂为第三类容积点的权值；

其中，e_p为第p个元素为1的n维单位向量；为e_p的正中点集合的第j个向量，为e_p的负中点集合的第j个向量；p＝1,2,…,n；

3)通过状态方程公式(4)传播容积点：

计算k时刻状态的预测值及k时刻状态误差协方差阵的预测值P_k|k-1：

(三)量测更新：

1)对P_k|k-1进行Cholesky分解：

2)计算容积点：

其中，i＝1,2,…,n，j＝1,2,…,n(n-1)/2；

3)通过观测方程公式(5)计算容积点量测值

其中，i＝1,2,…,n，j＝1,2,…,n(n-1)/2；

计算k时刻量测的预测值

4)计算自相关协方差阵P_zz,k|k-1和互相关协方差阵P_xz,k|k-1：

5)滤波增益计算：

6)计算状态估计值和状态误差协方差阵P_k：

的第二个变量即为提取得到的高低惯性视线角速度，第四个变量为提取得到的方位惯性视线角速度，由此获得捷联导引头制导信息。