一种基于交互式多模型卡尔曼滤波的无人机航迹跟踪方法

本发明属于航迹跟踪，具体涉及一种基于交互式多模型卡尔曼滤波的无人机航迹跟踪方法。

背景技术：

1、无人机作为引领低空经济发展的主体，在民用领域中得到了广泛的应用。在诸如工业巡检、物资运送等场景中，无人机通过应用自主飞行技术，可沿预先规划好的路径执行飞行任务；与此同时，保障对实时飞行无人机的持续监控对空域安全与地面安全至关重要，传统的监测手段如雷达、卫星通信以及地面移动网络存在诸多限制因素，包括成本、延迟地形条件等，给实时无人机飞行态势监测带来挑战。

2、为解决上述难题，具有低时延、低成本、易部署等突出优点的ads-b是可选的技术手段。但ads-b通常应用于民航领域，与民航客机相比，无人机体积小且机动性高，因此对于定位误差精度要求更高。

技术实现思路

1、本发明的目的在于克服现有技术中的不足，提供一种基于交互式多模型卡尔曼滤波的无人机航迹跟踪方法，跟踪的精度更高。

2、本发明提供了如下的技术方案：

3、提供了一种基于交互式多模型卡尔曼滤波的无人机航迹跟踪方法，包括：

4、构建无人机恒定速度模型以及无人机恒定急加速度模型；

5、根据机载ads-b系统，获取无人机飞行状态数据，并确定当前时刻无人机航迹的初始观测状态向量；

6、根据当前时刻无人机初始观测状态向量，利用无人机恒定速度模型，确定无人机第一观测状态向量，利用无人机恒定急加速度模型，确定无人机第二观测状态向量；

7、根据第一观测状态向量、第二观测状态向量和观测噪声协方差矩阵，使用两个卡尔曼滤波器并行执行基于无人机恒定速度模型和基于无人机恒定急加速度模型的预测和更新，确定处于恒定速度下的第一后验状态向量估计值以及处于恒定急加速度下的第二后验状态向量估计值；

8、将第一后验状态向量估计值和第二后验状态向量估计值进行结合，确定当前时刻的最终状态向量估计值，并将当前时刻的最终状态向量估计值作为无人机当前时刻的位置，以实现无人机航迹的跟踪。

9、可选地，所述构建无人机恒定速度模型以及无人机恒定急加速度模型时，

10、根据前一时刻的处于恒定速度的无人机状态向量、当前时间窗的恒定速度状态转移矩阵和恒定速度过程噪声协方差，构建无人机恒定速度模型；

11、根据前一时刻的处于恒定急加速度的无人机状态向量、以及当前时间窗的恒定急加速度状态转移矩阵和恒定急加速度过程噪声协方差，构建无人机恒定急加速度模型。

12、可选地，所述当前时间窗的恒定速度过程噪声协方差、所述当前时间窗的恒定急加速度过程噪声协方差和所述观测噪声协方差矩阵，均通过循环神经网络结构进行特征提取后生成。

13、所述循环神经网络结构有多层，其中第一层为lstm层用于捕获输入特征，第二层为全连接层用于映射第一层lstm隐藏层状态维度至指定维度，第三层为激活函数层，采用prelu激活函数为神经网络引入非线性；第四层为三个并列的全连接层用于将第三层的状态维度映射为指定维度，并输出三组特征提取参数；

14、根据循环神经网络结构的三组特征提取参数，生成当前时间窗的恒定速度过程噪声协方差、所述当前时间窗的恒定急加速度过程噪声协方差和所述观测噪声协方差矩阵；

15、所述循环神经网络结构的输入特征为每一时刻的第一观测状态向量与最终状态向量估计值的差值和每一时刻的第二观测状态向量与最终状态向量估计值的差值。

16、可选地，所述构建无人机恒定速度模型以及无人机恒定急加速度模型中，

17、所述无人机恒定速度模型为：

18、

19、其中，ok1为第一观测状态向量，hcv为恒定速度模型中的系统观测矩阵，为k时刻处于恒定速度的状态转移向量，vk为服从观测噪声协方差矩阵r的高斯白噪声；

20、所述无人机恒定急加速度模型为：

21、

22、其中，ok2为第一观测状态向量，hcj为恒定急加速度模型中的系统观测矩阵，为处于恒定急加速度的状态转移向量，vk为服从观测噪声协方差矩阵r的高斯白噪声；

23、所述观测噪声协方差矩阵r为：

24、

25、其中，分别表示在x,y和z方向上位置测量的不确定度，分别表示在x,y和z方向上速度测量的不确定度。

26、可选地，所述构建无人机恒定速度模型中，所述k时刻处于恒定速度的状态转移向量为：

27、

28、其中，为服从恒定速度过程噪声协方差矩阵qcv的高斯白噪声，为k-1时刻恒定速度的状态转移向量，fcv为恒定速度状态转移矩阵；

29、所述恒定速度状态转移矩阵fcv为：

30、

31、所述恒定速度过程噪声协方差矩阵qcv为：

32、

33、其中，为恒定速度模型下速度估计不确定度矩阵，t为ads-b进行数据广播的周期，i表示单位矩阵；

34、所述恒定速度模型中速度估计不确定度矩阵为：

35、

36、其中，分别表示在x,y和z方向上速度测量的不确定度；

37、所述恒定速度模型中的系统观测矩阵hcv为：

38、hcv＝diag(1,1,1,1,1,1)。

39、可选地，所述构建无人机恒定急加速度模型中，所述k时刻处于恒定急加速度的状态转移向量为：

40、

41、其中，表示服从恒定急加速度过程噪声协方差矩阵qcj的高斯白噪声，fcj为恒定急加速度状态转移矩阵，为k-1时刻恒定急加速度的状态转移向量；

42、所述恒定急加速度状态转移矩阵fcj为：

43、

44、其中，t为ads-b进行数据广播的周期，i表示单位矩阵；

45、所述恒定急加速度过程噪声协方差矩阵qcj为：

46、

47、其中，为恒定急加速度模型中速度估计不确定度矩阵；

48、所述恒定急加速度模型中速度估计不确定度矩阵为：

49、

50、其中，分别表示恒定急加速度模型下对于x,y和z方向上加速度的变化率估计的不确定度；

51、所述恒定急加速度模型中的系统观测矩阵hcj为：

52、

53、可选地，所述根据第一观测状态向量、第二观测状态向量和观测噪声矩阵，使用两个卡尔曼滤波器并行执行基于无人机恒定速度模型和基于无人机恒定急加速度模型的预测和更新，确定处于恒定速度下的第一后验状态向量估计值以及处于恒定急加速度下的第二后验状态向量估计值包括：

54、根据恒定速度模型上一时刻的后验概率和第一后验状态向量估计值，确定第一个卡尔曼滤波器的输入状态向量和输入估计误差协方差矩阵，并使用第一个卡尔曼滤波器在预测阶段确定恒定速度模型在当前时刻的第一先验状态估计值和第一先验估计协方差矩阵，在更新阶段确定恒定速度模型在当前时刻的第一后验状态向量估计值；

55、根据恒定急加速度模型上一时刻的后验概率和第二后验状态向量估计值，确定第二个卡尔曼滤波器的输入状态向量和输入估计误差协方差矩阵，并使用第二个卡尔曼滤波器在预测阶段确定恒定急加速度模型在当前时刻的第二先验状态估计值和第二先验估计协方差矩阵，在更新阶段确定恒定急加速度模型在当前时刻的第二后验状态向量估计值。

56、可选地，所述根据恒定速度模型上一时刻的后验概率和第一后验状态向量估计值，确定第一个卡尔曼滤波器的输入状态向量和输入估计误差协方差矩阵，并使用第一个卡尔曼滤波器在预测阶段确定恒定速度模型在当前时刻的第一先验状态估计值和第一先验估计协方差矩阵，在更新阶段确定恒定速度模型在当前时刻的第一后验状态向量估计值中，

57、第一个卡尔曼滤波器k时刻的输入状态向量和输入估计误差协方差矩阵p01,k∣k-1为：

58、

59、

60、其中，为k时刻恒定速度模型的输入模型概率；为k-1时刻第一后验状态向量估计值，pi,k-1∣k-1为k-1时刻第一后验估计误差协方差矩阵；

61、第一个卡尔曼滤波器在预测阶段：

62、k时刻的第一先验状态估计值和第一先验估计误差协方差矩阵p1,k∣k-1为：

63、

64、

65、其中，fcv为恒定速度状态转移矩阵，为k时刻的恒定速度过程噪声协方差矩阵；

66、第一个卡尔曼滤波器在更新阶段：

67、k时刻的第一后验状态估计值为：

68、

69、其中，为恒定速度模型下卡尔曼增益，ok1为第一观测状态向量，hcv为恒定速度模型中的系统观测矩阵；

70、所述恒定速度模型下卡尔曼增益为：

71、

72、其中，r为观测噪声协方差矩阵；

73、更新后在k时刻第一个卡尔曼滤波器的模型概率为：

74、

75、其中，为第一个滤波器的似然函数，为第n个滤波器的似然函数，λi1为第一个滤波器模型转移概率，为k-1时刻第一个卡尔曼滤波器的模型概率；

76、所述根据恒定急加速度模型上一时刻的后验概率和第二后验状态向量估计值，确定第二个卡尔曼滤波器的输入状态向量和输入估计误差协方差矩阵，并使用第二个卡尔曼滤波器在预测阶段确定恒定急加速度模型在当前时刻的第二先验状态估计值和第二先验估计协方差矩阵，在更新阶段确定恒定急加速度模型在当前时刻的第二后验状态向量估计值中，

77、第二个卡尔曼滤波器k时刻的输入状态向量和输入估计误差协方差矩阵p02,k∣k-1为：

78、

79、

80、其中，为k时刻恒定急加速度模型的输入模型概率；为k-1时刻第一后验状态向量估计值，pi,k-1∣k-1为k-1时刻第一后验估计误差协方差矩阵；

81、第二个卡尔曼滤波器在预测阶段：

82、k时刻的第二先验状态估计值和第二先验估计误差协方差矩阵p2,k∣k-1为：

83、

84、

85、其中，fcj为恒定急加速度状态转移矩阵，为k时刻的恒定急加速度过程噪声协方差矩阵；

86、第二个卡尔曼滤波器在更新阶段：

87、k时刻的第二后验状态估计值为：

88、

89、其中，为恒定急加速度模型下卡尔曼增益，ok2为第二观测状态向量，hcj为恒定急加速度模型中的系统观测矩阵；

90、所述恒定急加速度模型下卡尔曼增益为：

91、

92、其中，r为观测噪声协方差矩阵；

93、更新后第二个卡尔曼滤波器的模型概率为：

94、

95、其中，为第二个滤波器的似然函数，λi2为第二个滤波器模型转移概率，为k-1时刻的第二个卡尔曼滤波器的模型概率。

96、可选地，所述确定第一个卡尔曼滤波器的输入状态向量和输入估计误差协方差矩阵中，需要先确定恒定速度模型的输入模型概率；所述确定第二个卡尔曼滤波器的输入状态向量和输入估计误差协方差矩阵中，需要先确定恒定急加速度模型的输入模型概率；

97、所述确定恒定速度模型的输入模型概率以及确定恒定急加速度模型的输入模型概率均通过以下公式求得：

98、

99、其中，使用下标i,j,n表示滤波器动态模型，不同取值1，2分别表示恒定速度模型与恒定急加速度模型；为混合模型概率用于为不同模型滤波器准备初始状态，λij为模型转移概率，为上一时刻k-1的模型i的模型概率，为上一时刻k-1的第n个卡尔曼滤波器的模型概率。

100、可选地，所述将第一后验状态向量估计值和第二后验状态向量估计值进行结合，确定当前时刻的最终状态向量估计值，并将当前时刻的最终状态向量估计值作为无人机当前时刻的位置中：

101、所述当前时刻的最终状态向量估计值为：

102、

103、其中，为第n个后验状态向量估计值，为第n个卡尔曼滤波器k时刻的模型概率。

104、与现有技术相比，本发明的有益效果是：

105、本发明使用ads-b作为无人机的定位数据源，充分利用了ads-b在获取无人机飞信态势信息中的灵活性与实时性优势，提高了地面基站对无人机飞行状态的实时性掌控能力；本发明充分利用循环神经网络在处理时序数据时，性能相较于其他神经网络具有更好的精度和善于学习到时序数据间的隐藏关系，通过利用循环神经网络实现动态调节传统交互式多模型卡尔曼滤波中噪声矩阵的设置，有利于提升传统方法系统动态模型建模精度并有效适应观测环境中变化的噪声水平，相较于传统航迹跟踪算法，实现了更高的跟踪精度。