题目

巡航导弹沿直线飞向目标，目标处设有一监视雷达，雷达对导弹的距离进行观测。
假设：（1）导弹初始距离100km100km100km，速度约为300m/s300m/s300m/s，基本匀速飞行，但受空气扰动影响，扰动加速度为零均值白噪声，方差强度q=0.05m2/s3q=0.05m^2/s^3q=0.05m2/s3 ；（2）雷达观测频率2Hz2Hz2Hz，观测误差为零均值白噪声，均方差为50m50m50m。
试使用卡尔曼滤波（Kalman filter）完成导弹的轨迹跟踪。

matlab仿真代码

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 功能描述：导弹运动Kalman滤波程序
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear;clc;close all;
%% 01 初始化参数
T = 300;Ts = 1/2;               % 采样时间
Q = 0.05*Ts;            % 过程噪声
R = 50/Ts;              % 量测噪声
W = sqrt(Q)*randn(1,T);
V = sqrt(R)*randn(1,T);
P0 = diag([10^2,1^2]);  % 系统矩阵
Phi = [1,-Ts;0,1];
H = [1,0];
Tau = [0.5*Ts^2;Ts];% 初始化
nS = 2; nZ = 1;
xState = zeros(nS,T);
zMea = zeros(nZ,T);
xKF = zeros(nS,T);
xKF_pre = zeros(nS,T);
xState(:,1) = [100000;300];
zMea(:,1) = H*xState(:,1);
xKF(:,1) = xState(:,1);
%% 02 用模型模拟真实状态
for t = 2:TxState(:,t) = Phi*xState(:,t-1) + Tau*W(:,t);zMea(:,t) = H*xState(:,t) + V(t);
end
%% 03 Kalman滤波
for t = 2:TxKF_pre(:,t) = Phi*xKF(:,t-1);P_pre = Phi*P0*Phi' + Tau*Q*Tau';K = P_pre*H'*pinv(H*P_pre*H'+R);xKF(:,t) = xKF_pre(:,t) + K*(zMea(:,t)-H*xKF_pre(:,t));P0 = (eye(2)-K*H)*P_pre;
end
%% 04 画图
tPlot = 1:T;
FigWin1=figure('position',[300 400 550 450],'Color',[0.8 0.8 0.8],...'Name','01-量测距离误差与估计距离误差的比较','NumberTitle','off');hold on;box on;
plot(tPlot,xState(1,:)-zMea(1,:),'-b','LineWidth',1.5);hold on;
plot(tPlot,xState(1,:)-xKF(1,:),'-r','LineWidth',1.5);hold on;
xlabel('时间 t/s');ylabel('误差距离 m');
legend('量测距离误差','估计距离误差');
title('量测距离误差与估计距离误差的比较');FigWin2=figure('position',[850 400 550 450],'Color',[0.8 0.8 0.8],...'Name','02-真实速度与估计速度的比较','NumberTitle','off');hold on;box on;
plot(tPlot,xState(2,:)-xKF(2,:),'-b','LineWidth',1.5);hold on;
xlabel('时间 t/s');ylabel('误差速度 m/s');
title('真实速度与估计速度的比较');FigWin3=figure('position',[300 200 550 450],'Color',[0.8 0.8 0.8],...'Name','03-导弹真实轨迹与雷达量测','NumberTitle','off');hold on;box on;
plot(tPlot,xState(1,:),'-b','LineWidth',1.5);hold on;
plot(tPlot,zMea(1,:),'-r','LineWidth',1.5);hold on;
xlabel('时间 t/s');ylabel('距离 m');
legend('导弹真实轨迹','雷达量测轨迹');
title('导弹真实轨迹与雷达量测轨迹');FigWin4=figure('position',[850 200 550 450],'Color',[0.8 0.8 0.8],...'Name','04-估计距离与估计速度','NumberTitle','off');hold on;box on;
subplot(211);
plot(tPlot,xKF(1,:),'-b','LineWidth',1.5);hold on;
xlabel('时间 t/s');ylabel('估计距离 m');
subplot(212);
plot(tPlot,xKF(2,:),'-r','LineWidth',1.5);hold on;
xlabel('时间 t/s');ylabel('估计速度 m/s');
title('估计距离与估计速度');

仿真结果图如下所示。

python仿真代码

"""
#-------------------------------------------------------------------#
#                     Li Lingwei's Python Code                      #
#-------------------------------------------------------------------#
#                                                                   #
#                   @Project Name : Matlab to python                #
#                   @File Name    : Kalman.py                       #
#                   @Programmer   : Li Lingwei                      #
#                   @Start Date   : 2021/4/18                       #
#                   @Last Update  : 2021/4/26                       #
#-------------------------------------------------------------------#
# Function:                                                         #
#                                                                   #
#-------------------------------------------------------------------#
"""
import numpy as np
import pylab as plt"""01 初始化参数"""
T = 300Ts = 1 / 2  # 采样时间
Q = 0.05 * Ts  # 过程噪声
R = 50 / Ts  # 量测噪声
W = np.sqrt(Q) * np.random.randn(1, T)
V = np.sqrt(R) * np.random.randn(1, T)
P0 = np.diag([10 ** 2, 1 ** 2])# 系统矩阵
Phi = np.mat([[1, -Ts],[0, 1]])
H = np.mat([1, 0])
Tau = np.mat([[0.5 * (Ts ** 2)],[Ts]])# 初始化
nS = 2
nZ = 1
xState = np.zeros([nS, T])
xState = np.mat(xState)
zMea = np.zeros([nZ, T])
zMea = np.mat(zMea)
xKF = np.zeros([nS, T])
xKF = np.mat(xKF)
xKF_pre = np.zeros([nS, T])
xKF_pre = np.mat(xKF_pre)
xState[:, 0] = [[100000], [300]]
xKF[:, 0] = xState[:, 0]
zMea[:, 0] = H * xState[:, 0]"""02 用模型模拟真实状态"""
for t in range(1, T):xState[:, t] = Phi * xState[:, t - 1] + Tau * W[:, t]zMea[:, t] = H * xState[:, t] + V[:, t]"""03 Kalman滤波"""
for t in range(1, T):xKF_pre[:, t] = Phi * xKF[:, t - 1]P_pre = Phi * P0 * Phi.T + Tau * Q * Tau.TK = P_pre * H.T * np.linalg.pinv(H * P_pre * H.T + R)xKF[:, t] = xKF_pre[:, t] + K * (zMea[:, t] - H * xKF_pre[:, t])P0 = (np.identity(nS) - K * H) * P_pre"""04 画图"""
font = {'family': 'Times New Roman','weight': 'normal','size': 10}
fontTitle = {'family': 'Times New Roman','weight': 'normal','size': 12}
# 变成数组才可以画图
t = [t for t in range(1, T + 1)]
xStatePlot = xState.getA()
xKFPlot = xKF.getA()
zMeaPlot = zMea.getA()# 01 量测距离误差与估计距离误差的比较
plt.figure(1)
plt.plot(t, xStatePlot[0, :] - zMeaPlot[0, :], 'b', linewidth=1, label='Position error of measurement')
plt.plot(t, xStatePlot[0, :] - xKFPlot[0, :], 'r', linewidth=1, label='Position error of estimation')
plt.title('Position error', fontdict=fontTitle)
plt.ylabel('Position (m)', fontdict=font)
plt.tick_params(pad=3)
plt.yticks(fontproperties='Times New Roman', size=10)
plt.xticks(fontproperties='Times New Roman', size=10)
plt.xlim(0, T)
plt.legend(prop=font)# 02 真实速度与估计速度的比较
plt.figure(2)
plt.plot(t, xStatePlot[1, :] - xKFPlot[1, :], 'r', linewidth=1)
plt.title('Velocity error', fontdict=fontTitle)
plt.xlabel('Time (s)', fontdict=font), plt.ylabel('Velocity (m/s)', fontdict=font)
plt.tick_params(pad=3)
plt.yticks(fontproperties='Times New Roman', size=10)
plt.xticks(fontproperties='Times New Roman', size=10)
plt.xlim(0, T)# 03 导弹真实轨迹与雷达量测
plt.figure(3)
plt.plot(t, xStatePlot[0, :], 'b', linewidth=1, label='True trajectory')
plt.plot(t, zMeaPlot[0, :], 'r', linewidth=1, label='Measure trajectory')
plt.title('Velocity error', fontdict=fontTitle)
plt.xlabel('Time (s)', fontdict=font), plt.ylabel('Velocity (m/s)', fontdict=font)
plt.tick_params(pad=3)
plt.yticks(fontproperties='Times New Roman', size=10)
plt.xticks(fontproperties='Times New Roman', size=10)
plt.xlim(0, T)
plt.legend(prop={'family': 'Times New Roman', 'size': 10})# 04 估计距离与估计速度
plt.figure(4)
plt.subplot(2, 1, 1)
plt.plot(t, xKFPlot[0, :], 'b', linewidth=1)
plt.title('Position', fontdict=fontTitle)
plt.ylabel('Position (m)', fontdict=font)
plt.tick_params(pad=3)
plt.yticks(fontproperties='Times New Roman', size=10)
plt.xticks([], fontproperties='Times New Roman', size=10)
plt.xlim(0, T)plt.subplot(2, 1, 2)
plt.plot(t, xKFPlot[1, :], 'r', linewidth=1)
plt.title('Velocity', fontdict=fontTitle)
plt.xlabel('Time (s)', fontdict=font), plt.ylabel('Velocity (m/s)', fontdict=font)
plt.tick_params(pad=3)
plt.yticks(fontproperties='Times New Roman', size=10)
plt.xticks(fontproperties='Times New Roman', size=10)
plt.xlim(0, T)plt.show()

仿真结果图如下所示。

更新了一版函数形式的Kalman滤波python代码。

"""
#-------------------------------------------------------------------#
#                     Li Lingwei's Python Code                      #
#-------------------------------------------------------------------#
#                                                                   #
#                   @Project Name : Matlab to python                #
#                   @File Name    : Kaman_V2_0.py                   #
#                   @Programmer   : Li Lingwei                      #
#                   @Start Date   : 2021/4/26                       #
#                   @Last Update  : 2021/4/26                       #
#-------------------------------------------------------------------#
# Function:                                                         #
#               写成函数的形式实现Kalman滤波                          #
#-------------------------------------------------------------------#
"""
import numpy as np
import pylab as plt# 初始状态设定子程序
def initKF():T = 300                                         # 仿真总步长Ts = 1 / 2                                      # 采样时间Q = 0.05 * Ts                                   # 过程噪声R = 50 / Ts                                     # 量测噪声W = np.sqrt(Q) * np.random.randn(1, T)V = np.sqrt(R) * np.random.randn(1, T)P0 = np.diag([10 ** 2, 1 ** 2])# 系统矩阵Phi = np.mat([[1, -Ts],[0, 1]])H = np.mat([1, 0])Tau = np.mat([[0.5 * (Ts ** 2)],[Ts]])# 初始化nS = 2nZ = 1xState = np.zeros([nS, T])xState = np.mat(xState)zMea = np.zeros([nZ, T])zMea = np.mat(zMea)xKF = np.zeros([nS, T])xKF = np.mat(xKF)xKF_pre = np.zeros([nS, T])xKF_pre = np.mat(xKF_pre)xState[:, 0] = [[100000], [300]]xKF[:, 0] = xState[:, 0]zMea[:, 0] = H * xState[:, 0]return T, Ts, Q, R, W, V, P0, Phi, H, Tau, xState, zMea, xKF, xKF_pre, nS# 真实状态生成子程序
def trueState(xState, zMea, Phi, Tau, H, W, V, T):for t in range(1, T):xState[:, t] = Phi * xState[:, t - 1] + Tau * W[:, t]zMea[:, t] = H * xState[:, t] + V[:, t]return xState, zMea# Kalman滤波子程序
def KF(Phi, Tau, H, nS, xKF_pre, xKF, zMea, P0, Q, R, T):for t in range(1, T):xKF_pre[:, t] = Phi * xKF[:, t - 1]P_pre = Phi * P0 * Phi.T + Tau * Q * Tau.TK = P_pre * H.T * np.linalg.pinv(H * P_pre * H.T + R)xKF[:, t] = xKF_pre[:, t] + K * (zMea[:, t] - H * xKF_pre[:, t])P0 = (np.identity(nS) - K * H) * P_prereturn xKF# 画图子程序
def plotKF(xState, xKF, zMea, T):font = {'family': 'Times New Roman','weight': 'normal','size': 10}fontTitle = {'family': 'Times New Roman','weight': 'normal','size': 12}# 变成数组才可以画图t = [t for t in range(1, T + 1)]xStatePlot = xState.getA()xKFPlot = xKF.getA()zMeaPlot = zMea.getA()# 01 量测距离误差与估计距离误差的比较plt.figure(1)plt.plot(t, xStatePlot[0, :] - zMeaPlot[0, :], 'b', linewidth=1, label='Position error of measurement')plt.plot(t, xStatePlot[0, :] - xKFPlot[0, :], 'r', linewidth=1, label='Position error of estimation')plt.title('Position error', fontdict=fontTitle)plt.ylabel('Position (m)', fontdict=font)plt.tick_params(pad=3)plt.yticks(fontproperties='Times New Roman', size=10)plt.xticks(fontproperties='Times New Roman', size=10)plt.xlim(0, T)plt.legend(prop=font)# 02 真实速度与估计速度的比较plt.figure(2)plt.plot(t, xStatePlot[1, :] - xKFPlot[1, :], 'r', linewidth=1)plt.title('Velocity error', fontdict=fontTitle)plt.xlabel('Time (s)', fontdict=font), plt.ylabel('Velocity (m/s)', fontdict=font)plt.tick_params(pad=3)plt.yticks(fontproperties='Times New Roman', size=10)plt.xticks(fontproperties='Times New Roman', size=10)plt.xlim(0, T)# 03 导弹真实轨迹与雷达量测plt.figure(3)plt.plot(t, xStatePlot[0, :], 'b', linewidth=1, label='True trajectory')plt.plot(t, zMeaPlot[0, :], 'r', linewidth=1, label='Measure trajectory')plt.title('Velocity error', fontdict=fontTitle)plt.xlabel('Time (s)', fontdict=font), plt.ylabel('Velocity (m/s)', fontdict=font)plt.tick_params(pad=3)plt.yticks(fontproperties='Times New Roman', size=10)plt.xticks(fontproperties='Times New Roman', size=10)plt.xlim(0, T)plt.legend(prop={'family': 'Times New Roman', 'size': 10})# 04 估计距离与估计速度plt.figure(4)plt.subplot(2, 1, 1)plt.plot(t, xKFPlot[0, :], 'b', linewidth=1)plt.title('Position', fontdict=fontTitle)plt.ylabel('Position (m)', fontdict=font)plt.tick_params(pad=3)plt.yticks(fontproperties='Times New Roman', size=10)plt.xticks([], fontproperties='Times New Roman', size=10)plt.xlim(0, T)plt.subplot(2, 1, 2)plt.plot(t, xKFPlot[1, :], 'r', linewidth=1)plt.title('Velocity', fontdict=fontTitle)plt.xlabel('Time (s)', fontdict=font), plt.ylabel('Velocity (m/s)', fontdict=font)plt.tick_params(pad=3)plt.yticks(fontproperties='Times New Roman', size=10)plt.xticks(fontproperties='Times New Roman', size=10)plt.xlim(0, T)plt.show()if __name__ == '__main__':"""01 初始化参数"""T, Ts, Q, R, W, V, P0, Phi, H, Tau, xState, zMea, xKF, xKF_pre, nS = initKF()"""02 用模型模拟真实状态"""xState, zMea = trueState(xState, zMea, Phi, Tau, H, W, V, T)"""03 Kalman滤波"""xKF = KF(Phi, Tau, H, nS, xKF_pre, xKF, zMea, P0, Q, R, T)"""04 画图"""plotKF(xState, xKF, zMea, T)

−−−End−−−---End---−−−End−−−

用卡尔曼滤波器跟踪导弹相关推荐

用卡尔曼滤波器跟踪导弹（量测更新频率与时间更新频率不相等）
题目巡航导弹沿直线飞向目标,目标处设有一监视雷达,雷达对导弹的距离进行观测. 假设:(1)导弹初始距离 100 k m 100km 100km,速度约为 300 m / s 300m/s 300m/ ...
【滤波】设计卡尔曼滤波器
%matplotlib inline #format the book import book_format book_format.set_style() 简介在上一章节中,我们讨论了教科书式的问 ...
智慧交通day02-车流量检测实现08：目标跟踪中的数据关联(将检测框bbox与卡尔曼滤波器的跟踪框进行关联匹配)
# 将YOLO模型的检测框和卡尔曼滤波的跟踪框进行匹配 def associate_detection_to_tracker(detections,trackers,iou_threshold=0.3 ...
基于MATLAB卡尔曼滤波器实现动态人物的跟踪检测
卡尔曼滤波不要求信号和噪声都是平稳过程的假设条件.对于每个时刻的系统扰动和观测误差(即噪声),只要对它们的统计性质作某些适当的假定,通过对含有噪声的观测信号进行处理,就能在平均的意义上,求得误差为最小 ...
卡尔曼滤波器（目标跟踪一）（上）
文章目录单目标检测 IOU跟踪目标丢失人群密集卡尔曼滤波直观理解状态方程噪声协方差卡尔曼公式案例运用本文主要是针对目标跟踪算法进行一个学习编码,从比较简单的卡尔曼滤波器开始,到后面 ...
ad 卡尔曼_对Kalman(卡尔曼)滤波器的理解
1．简介(Brief Introduction) 在学习卡尔曼滤波器之前,首先看看为什么叫"卡尔曼".跟其他著名的理论(例如傅立叶变换,泰勒级数等等)一样,卡尔曼也是一个人的名字, ...
卡尔曼滤波器(Kalman Filter) 理解
卡尔曼滤波器 1 简介(Brief Introduction) 在学习卡尔曼滤波器之前,首先看看为什么叫"卡尔曼".跟其他著名的理论(例如傅立叶变换,泰勒级数等等)一样,卡尔曼也是 ...
Udacity机器人软件工程师课程笔记（三十二) - 卡尔曼滤波器 - 一维卡尔曼滤波器 - 多维卡尔曼滤波器 - 拓展卡尔曼滤波器（EKF）
卡尔曼滤波器一.概述二.一维高斯分布均值和方差三.一维卡尔曼滤波器变量命名约定卡尔曼滤波循环 1.测量值更新 (1)平均值计算 (2)程序实现 2.位置预测位置预测公式 3.一维卡尔曼滤 ...
面向软件工程师的卡尔曼滤波器
点击上方"小白学视觉",选择加"星标"或"置顶" 重磅干货,第一时间送达本文转自|磐创AI 与我的朋友交谈时,我经常听到:"哦, ...

用卡尔曼滤波器跟踪导弹

题目

matlab仿真代码

python仿真代码

用卡尔曼滤波器跟踪导弹相关推荐

最新文章

热门文章