(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211117541.3 (22)申请日 2022.09.14 (71)申请人 航天恒星科技有限公司 地址 100086 北京市海淀区知春路82号院 (72)发明人 杨伟才　于雪晖　侯旭涛　田广宇　 李青松　任文　 (74)专利代理 机构 中国航天科技专利中心 11009 专利代理师 高志瑞 (51)Int.Cl. G01S 5/02(2010.01) G01C 21/20(2006.01) G06F 17/16(2006.01) (54)发明名称 联合角度频率测量的扩展卡尔曼滤波定位 方法及系统 (57)摘要 本发明公开了一种联合角度频率测量的扩 展卡尔曼滤波定位方法及系统， 其中， 该方法包 括： 建立空间目标运动模型； 根据目标辐射源的 目标特征得到方位角观测量、 俯仰角观测量和频 率观测量； 根据空间目标运动模 型建立状态预测 方程与误差协方差预测方程； 根据方位角观测 量、 俯仰角观测量和频率观测量 建立线性化形式 的测量方程； 根据状态预测方程、 误差协方差预 测方程和测量方程得到卡尔曼滤波增益， 根据卡 尔曼滤波增益得到滤波后的状态变量和滤波后 的协方差矩阵。 本发明提升 了定位的可靠性。 权利要求书3页 说明书10页 附图1页 CN 115542244 A 2022.12.30 CN 115542244 A 1.一种联合角度频率测量的扩展卡尔曼 滤波定位方法， 其特 征在于包括： 建立空间目标运动模型； 根据目标辐射源的目标 特征得到方位角观测量、 俯仰角观测量和频率观测量； 根据空间目标运动模型建立状态预测方程与误差协方差预测方程； 根据方位角观测 量、 俯仰角观测量和频率观测量建立线性 化形式的测量方程； 根据状态预测方程、 误差协方差预测方程和测量方程得到卡尔曼滤波增益， 根据卡尔 曼滤波增益得到滤波后的状态变量和滤波后的协方差矩阵。 2.根据权利要求1所述的联合角度频率测量的扩展卡尔曼滤波定位方法， 其特征在于： 空间目标运动模型通过如下公式得到： 其中， Xi为i时刻状态， Xi‑1为i‑1时刻状态， Ai‑1为i‑1时刻转移矩阵， μ为地心引力常数， i 为时间， r为 卫星到地心的距离， T为观测器的采样间隔。 3.根据权利要求1所述的联合角度频率测量的扩展卡尔曼滤波定位方法， 其特征在于： 方位角观测量 通过如下公式得到： 其中， β 为方位角观测量， xe为辐射源x方向坐标， xcub为观测器x方向坐标， ye为辐射源y 方向坐标， ycub为观测器y方向坐标， δβ为方位角测量 误差。 4.根据权利要求1所述的联合角度频率测量的扩展卡尔曼滤波定位方法， 其特征在于： 俯仰角观测量 通过如下公式得到： 其中， ε为俯仰角观测量， xe为辐射源x方向坐标， xcub为观测器x方向坐标， ye为辐射源y 方向坐标， ycub为观测器y方向坐标， ze为辐射源z方向坐标， zcub为观测器z方向坐标， δε为俯 仰角测量 误差。 5.根据权利要求1所述的联合角度频率测量的扩展卡尔曼滤波定位方法， 其特征在于： 频率观测量 通过如下公式得到： 其中， f为频率观测量， f0为目标辐射源信号的载波频率， fd为多普勒频率, 为目标的权　利　要　求　书 1/3 页 2 CN 115542244 A 2三维速度， 为观测器的三维速度， pe为辐射源三维位置坐标， pcub为观测器三维位置坐 标， c为光速， δf为频率测量 误差。 6.根据权利要求1所述的联合角度频率测量的扩展卡尔曼滤波定位方法， 其特征在于： 状态预测方程表达为： 其中， 为i时刻状态预测值， 为i‑1时刻状态矩阵， ωi‑1为系统噪声， μ为地 心引力常数， i 为时间， r为 卫星到地心的距离， T为观测器的采样间隔， AAF为状态转移 矩阵。 7.根据权利要求1所述的联合角度频率测量的扩展卡尔曼滤波定位方法， 其特征在于： 误差协方差预测方程表达为： 其中， AAF为状态转移矩阵， 为误差协方差预测值， 为i‑1时刻误差协方差 矩阵， AAF’为状态转移 矩阵的转置矩阵， Q 为过程噪声协方差 。 8.根据权利要求1所述的联合角度频率测量的扩展卡尔曼滤波定位方法， 其特征在于： 线性化形式的测量方程表达为： 其中， 为测量方程的线性化矩阵， Z为测量矩阵， XAF为状态矩阵， 为辐射源x方 向坐标， 为辐射源y方向坐标， 为辐射源z方向坐标， vxe为辐射源x 方向速度， vye为辐 射源y方向速度， vze为辐射源 z方向速度， β 为方位角观测量， ε为俯仰角观测量， f为频率观测 量， f0为目标辐射源信号的载波频率。 9.根据权利要求1所述的联合角度频率测量的扩展卡尔曼滤波定位方法， 其特征在于： 卡尔曼滤波增益 通过如下公式得到：权　利　要　求　书 2/3 页 3 CN 115542244 A 3