【多传感器融合】基于卡尔曼、无迹卡尔曼、拓展卡尔曼、粒子滤波实现非移动 GPS 干扰器的多传感器融合和位置估计附matlab代码
【多传感器融合】基于卡尔曼、无迹卡尔曼、拓展卡尔曼、粒子滤波实现非移动 GPS 干扰器的多传感器融合和位置估计附matlab代码
TT_Matlab
博主简介：擅长智能优化算法、神经网络预测、信号处理、元胞自动机、图像处理、路径规划、无人机等多种领域的Matlab仿真，完整matlab代码或者程序定制加qq1575304183。
✅作者简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，matlab项目合作可私信。
个人主页： Matlab科研工作室
个人信条：格物致知。
更多Matlab仿真内容点击
智能优化算法 神经网络预测 雷达通信 无线传感器 电力系统
信号处理 图像处理 路径规划 元胞自动机 无人机
⛄ 内容介绍
GPS Jammer Localisation Methods
The position of the GPS jamming vehicle is not directly observable and must be therefore observed by utilizing obseravble measurements. In order to localise the target that is confusing the GPS signals, a power measurement of the jamming signal is used. This method is called the Received signal strength (RSS) method, which is often used for localisation of energy emitting sources.
UAV Guidance Approach
The guidance approach which is utilised for the UAV guidance is a vector field based path following. In which the UAV first follow a straight line approach to the estimated position of the target. When the UAV reaches a certain distance it starts to follow a loiter path.
Sensor Fusion Algorithms
The goal of the assignment was it to develop and implement different sensor fusion algorihtms for both an isotropic and anistropic GPS jammer pattern. The following algorhtms were implemented:
Extended Kalman Filter
Unscendet Kalman Filter
Particle Filter
Extended Particle Filter
Unscendet Particle Filter
H-Infinity Filter
Adaptive Kalman Filter
H-Infinity Particle Filter
Particle Filter Resampling
A set of potential resampling approaches have been implmented. The goal of that analysis was to compare the different performances of the resampling methods in the same environment and to choose the algorithm with the best performance. Following methods have been implemented and investigated:
Multinomial Resampling
Systemematic Resampling
Residual Resampling
Residual Systematic Resampling
Local Selection Resampling
Stratified Resampling
⛄ 部分代码
% Main file for first geolocation simulation: isotropic static jammer
% -----------------------------------------------------------------------
% ------------------------ Algorithm ------------------------------
% -----------------------------------------------------------------------
% The algorithm goes through: prompts & constants, simulation parameters,
% variable initialisation, main loop, error checking and plotting
der+1) % If simulation has initialised
P_r_filt_ratio(k,1)=((P_r_filt(k,1)))/(P_r_filt(1,1)); % Get power ratio: alpha
if (abs((P_r_filt_ratio(k,1)-1))>0.05/100) % If ratio away from 1 with confidence
[centre_geo_circle(k,:) radius_geo_circle(k,1)]=get_geo_data(x_vec_all(1,:),x_vec_all(k,:),P_r_filt_ratio(k,1)); % See corresponding function
else
alpha_eq_1=1; % Boolean to indicate that ratio is close to 1 therefore set to 1 in the simulation
end
end
% Kalman filtering: EKF (UKF)
% First stage: intersection check
if ((obs_check==0)&&(k>k_C_prim)&&(d_uav(k,1)-d_uav(k_C_prim,1)>50))% If intersection is not true yet and UAV has travelled a small distance
obs_condtn=get_obs_condtn(centre_geo_circle(k_C_prim,1),centre_geo_circle(k_C_prim,2),centre_geo_circle(k,1),centre_geo_circle(k,2),radius_geo_circle(k_C_prim,1),radius_geo_circle(k,1));
if (obs_condtn>0) % Circles begin to intersect
obs_check=1; % EKF (UKF) may start is observable
k_obs=k+(floor((2/100)*N_loops_fb)+1); % Add safety margin for geometry to change
end
k_C_prim=k; % Update k_C_prim for next distance check
end
% Calls to the EKF(UKF)
if (((obs_check==1)&&(k==k_obs))||(re_run_bool==1)) % If intersections have begun & first time EKF (UKF) is run
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%---------- < Design a EKF (UKF) > ------------%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% To make sure this file run, I just put x_state(:,k) = x_t_vec
% (the true target postion)
%x_state(:,k) = x_t_vec;
%%% students must uncomment the following line and design a EKF
%%% and UKF
[x_state(:,k),P_cov(:,:,k),K_EKF_gain(:,k)]=UPF_form(x_vec_all(1,:),x_vec_all(k,:),h_0,P_r_filt_ratio(k,1),x_state_ini,P_cov_ini,F_KF,G_KF,Q_KF,R_KF);
if (re_run_bool==1)
re_run_bool=0;
div_EKF_bool=0;
end
elseif ((obs_check==1)&&(k>k_obs)) % If intersections have begun & EKF (UKF) has alreay started
% To make sure this file run, I just put x_state(:,k) = x_t_vec
% (the true target postion)
%x_state(:,k) = x_t_vec;
%%% students must uncomment the following line and design a EKF
%%% and UKF
[x_state(:,k),P_cov(:,:,k),K_EKF_gain(:,k)]=UPF_form(x_vec_all(1,:),x_vec_all(k,:),h_0,P_r_filt_ratio(k,1),x_state_ini,P_cov_ini,F_KF,G_KF,Q_KF,R_KF);
end
% RMS EKF(UKF) calculation and P_t estimation
% Animation: plot new UAV, Jammer and UAV trace at each iteration.
% See corresponding function for detail
plot_animation_search(N_plots,k,x_t_vec,x_vec_all(1:k,:),psi_all(k,1),r_est_l(k,1),r_est_h(k,1),centre_geo_circle(k,:),radius_geo_circle(k,1),x_state(:,1:k),k_obs,N_loops_fb,P_cov(:,:,k),p_e,0,psi_jammer);
end
% ------------------------ End Main flyby loop -----------------------------
%%
% -----------------------------------------------------------------------
% ------------------- Flyby Results Analysis -----------------------
% -----------------------------------------------------------------------
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%% Students must analyse the performance of their own filters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%
% -----------------------------------------------------------------------
% -- Initialise Vector field simulation parameters from flyby phase -
% -----------------------------------------------------------------------
% This part consists in augmenting previous matrices from flyby with
% initialised parameters (zeros mostly)
% UAV
x_vec_all=[x_vec_all ; zeros(N_loops_vf-N_loops_fb,2)]; % Initialise uav position vector
x_vec_all(N_loops_fb+1,:)=x_vec_all(N_loops_fb,:)+D_T*x_vec_dot(N_loops_fb,:); % Initialise for first step
x_vec_dot=[x_vec_dot ; zeros(N_loops_vf-N_loops_fb,2)]; % Initialise uav velocity vector: derivative of x_vec_all
psi_all=[psi_all ; zeros(N_loops_vf-N_loops_fb,1)]; % Initialise uav heading vector
psi_all(N_loops_fb+1,1)=psi_all(N_loops_fb,:)+D_T*psi_dot(N_loops_fb,:); % Initialise psi for first step
psi_dot=[psi_dot ; zeros(N_loops_vf-N_loops_fb,1)]; % Initialise uav heading derivative vector
jammer_UAV_vec_p=[jammer_UAV_vec_p ; zeros(N_loops_vf-N_loops_fb,3)]; % Initialise Jammer-->UAV vector (3D)
elev_angle=[elev_angle ; zeros(N_loops_vf-N_loops_fb,1)]; % Initialise elevation angle to vertical
azimuth_angle=[azimuth_angle ; zeros(N_loops_vf-N_loops_fb,1)]; % Initialise Jammer-->UAV azimuth angle to horizontal
azimuth_rel_angle=[azimuth_rel_angle ; zeros(N_loops_vf-N_loops_fb,1)]; % Initialise Jammer-->UAV azimuth angle to jammer direction
% Measurements
r_true=[r_true ; zeros(N_loops_vf-N_loops_fb,1)]; % True slant range
P_r_true=[P_r_true ; zeros(N_loops_vf-N_loops_fb,1)]; % True received power
P_r_meas=[P_r_meas ; zeros(N_loops_vf-N_loops_fb,1)]; % Measured received power: with noise
% Processing
r_est_l=[r_est_l ; zeros(N_loops_vf-N_loops_fb,1)]; % Lower range estimation
r_est_h=[r_est_h ; zeros(N_loops_vf-N_loops_fb,1)]; % Upper range estimation
r_est_unc=[r_est_unc ; zeros(N_loops_vf-N_loops_fb,1)]; % Range estimation uncertainty
P_r_filt_ratio=[P_r_filt_ratio ; zeros(N_loops_vf-N_loops_fb,1)]; % Alpha: Power ratio between initial and current : see ’alpha’ in report
centre_geo_circle=[centre_geo_circle ; zeros(N_loops_vf-N_loops_fb,2)]; % Centre of geolocation circle at instant k
radius_geo_circle=[radius_geo_circle ; zeros(N_loops_vf-N_loops_fb,1)]; % Radius of geolocation circle at instant k
% Filters
% EKF (UKF)
r_est=zeros(N_loops_vf,1); % Only used in the second part (VF)
x_state=[x_state zeros(2,N_loops_vf-N_loops_fb)]; % Updated EKF (UKF) state vector for all steps
P_cov(:,:,N_loops_fb+1:N_loops_vf)=0; % EKF (UKF) Covariance matrix for all
K_EKF_gain=[K_EKF_gain zeros(2,N_loops_vf-N_loops_fb)]; % Kalman gain storage
% Simulation data
d_uav=[d_uav ; zeros(N_loops_vf-N_loops_fb,1)]; % Distance travelled by the UAV
d_uav(N_loops_fb+1,1)=d_uav(N_loops_fb,:)+D_T*sqrt(x_vec_dot(N_loops_fb,:)*(x_vec_dot(N_loops_fb,:))’);
% Get Vector field orientation depending on UAV heading and azimuth to
% jammer
psi_uav=rem(psi_all(N_loops_fb,1),2*pi); % UAV heading (0 - 2pi)
beta_angle=psi_uav-azimuth_angle(N_loops_fb,1); % Angle between Jammer-->UAV and UAV heading
beta_angle = rem(beta_angle,2*pi); % psi_diff in [0 2*pi]
if abs(beta_angle)>pi
beta_angle = beta_angle-2*pi*sign(beta_angle);
end
if (beta_angle>=0)
VF_rot_sen=1; % Counter-clockwise
else
VF_rot_sen=-1; % Clockwise
end
%%
% -----------------------------------------------------------------------
% ------ Main Vector Field (VF) simulation Part --------------
% -----------------------------------------------------------------------
for k=(N_loops_fb+1):N_loops_vf
% UAV dynamics:
x_vec_dot(k,:)=V_g*[cos(psi_all(k,1)) sin(psi_all(k,1))]; % Update UAV speed vector with speed and heading
% Lyapunov vector field guidance (LVFG)
% Range estimation
r_est(k,1)=sqrt(((x_vec_all(k,1)-x_state(1,k-1))^2)+((x_vec_all(k,2)-x_state(2,k-1))^2)+(h_0^2)); % Equation 3.5 in report: range determination
% Vector field calculation
x_r=x_vec_all(k,1)-x_state(1,k-1); % relative x distance
y_r=x_vec_all(k,2)-x_state(2,k-1); % relative y distance
% Vector field component
f_1=(-alpha_f*V_g/r_est(k,1))*((x_r*((r_est(k,1)^2-r_d^2)/(r_est(k,1)^2+r_d^2)))+VF_rot_sen*(y_r*((2*r_est(k,1)*r_d)/(r_est(k,1)^2+r_d^2))));
f_2=(-alpha_f*V_g/r_est(k,1))*((y_r*((r_est(k,1)^2-r_d^2)/(r_est(k,1)^2+r_d^2)))-VF_rot_sen*(x_r*((2*r_est(k,1)*r_d)/(r_est(k,1)^2+r_d^2))));
% Desired heading
psi_d=atan2(f_2,f_1);
% Difference between current and desired
psi_diff=psi_all(k,1)-psi_d;
psi_diff = rem(psi_diff,2*pi); % psi_diff in [0 2*pi]
if abs(psi_diff)>pi
psi_diff = psi_diff-2*pi*sign(psi_diff);
end
% Desired heading rate
psi_dot_d=4*alpha_f*V_g*((r_d*r_true(k,1)^2)/((r_true(k,1)^2+r_d^2)^2));
% Turning rate command
psi_dot(k,1)=-K_LVFG_psi*psi_diff+psi_dot_d;
% Saturation check / UAV turn radius limit
if (abs(psi_dot(k,1))>(V_g/min_turn_r))
psi_dot(k,1)=(V_g/min_turn_r)*sign(psi_dot(k,1));
end
% UAV movement:
if k~=N_loops_vf % Not updated past (N_loops_vf-1)
x_vec_all(k+1,:)=x_vec_all(k,:)+D_T*x_vec_dot(k,:); % Position Euler integration
psi_all(k+1,:)=psi_all(k,:)+D_T*psi_dot(k,:); % Heading Euler integration
d_uav(k+1,:)=d_uav(k,:)+D_T*sqrt(x_vec_dot(k,:)*(x_vec_dot(k,:))’); % Travelled distance Euler integration
end
% UAV measurement:
r_true(k,1)=sqrt(((x_vec_all(k,1)-x_t_vec(1,1))^2)+((x_vec_all(k,2)-x_t_vec(1,2))^2)+(h_0^2));
% True Received power through Friis equation. However f varies
% slightly and the instrumentation measures P_r with some error
P_r_true(k,1)=(P_t*G_t*G_r*((c_0/(4*pi*r_true(k,1)*f_L1))^2)); % Equation 3.6 in report: Friis P_r in [W]
P_r_meas(k,1)=P_r_true(k,1)+sig_P_r_W*randn(1); % Add noise in W
% UAV measurement pre-processing
% Received power filtering
if (k>3*butter_order) % filter only works with sufficient data points
P_r_filt=zeros(k,1); % Re-Initialise filtered data at each step
P_r_filt(1:k,1)=filtfilt(b_butter,a_butter,P_r_meas(1:k,1)); % Filter noisy P_r_true at each new step
end
% Simulation data:
% Process measurements for geolocation
% First: process range determination
if (k>3*butter_order) % If simulation has enough point (filtering)
r_est_l(k,1)=((c_0/(4*pi*f_L1))*sqrt((G_t*G_r/(P_r_filt(k,1)))*P_t_min)); % Evaluate lower range from measurements
r_est_h(k,1)=((c_0/(4*pi*f_L1))*sqrt((G_t*G_r/(P_r_filt(k,1)))*P_t_max)); % Evaluate Upper range from measurements
r_est_unc(k,1)=abs(r_est_h(k,1)-r_est_l(k,1)); % Evaluate uncertainty on range measurements
end
% Second: process iso-(range ratio) curves
if (k>3*butter_order+1) % If simulation has initialised
P_r_filt_ratio(k,1)=((P_r_filt(k,1)))/(P_r_filt(1,1)); % Get power ratio: alpha
if (abs((P_r_filt_ratio(k,1)-1))>0.05/100) % If ratio away from 1 with confidence
[centre_geo_circle(k,:) radius_geo_circle(k,1)]=get_geo_data(x_vec_all(1,:),x_vec_all(k,:),P_r_filt_ratio(k,1)); % See corresponding function
else
alpha_eq_1=1; % Boolean to indicate that ratio is close to 1 therefore set to 1 in the simulation
end
end
% Kalman filtering: EKF (UKF)
if (re_run_bool==1) % If EKF (UKF) has diverged and needs to reinitialised
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%---------- < Design a EKF (UKF) > ------------%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% To make sure this file run, I just put x_state(:,k) = x_t_vec
% (the true target postion)
%x_state(:,k) = x_t_vec;
%%% students must uncomment the following line and design a EKF
%%% and UKF
[x_state(:,k),P_cov(:,:,k),K_EKF_gain(:,k)]=UPF_form(x_vec_all(1,:),x_vec_all(k,:),h_0,P_r_filt_ratio(k,1),x_state_ini,P_cov_ini,F_KF,G_KF,Q_KF,R_KF);
re_run_bool=0;
div_EKF_bool=0;
elseif (re_run_bool==0) % Normal operation condition: the EKF (UKF) has converged and remains on target
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%---------- < Design a EKF (UKF) > ------------%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% To make sure this file run, I just put x_state(:,k) = x_t_vec
% (the true target postion)
%x_state(:,k) = x_t_vec;
%%% students must uncomment the following line and design a EKF
%%% and UKF
[x_state(:,k),P_cov(:,:,k),K_EKF_gain(:,k)]=UPF_form(x_vec_all(1,:),x_vec_all(k,:),h_0,P_r_filt_ratio(k,1),x_state_ini,P_cov_ini,F_KF,G_KF,Q_KF,R_KF);
end
% Animation: plot new UAV, Jammer and UAV trace at each iteration.
% See corresponding function for detail
plot_animation_search(N_plots,k,x_t_vec,x_vec_all(1:k,:),psi_all(k,1),r_est_l(k,1),r_est_h(k,1),centre_geo_circle(k,:),radius_geo_circle(k,1),x_state(:,1:k),k_obs,N_loops_fb,P_cov(:,:,k),p_e,r_d,psi_jammer);
end
%%
% -----------------------------------------------------------------------
% ------------------------ Performance Check -------------------------
% -----------------------------------------------------------------------
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%% Students must analyse the performance of their own filters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
⛄ 运行结果
⛄ 参考文献
[1] A. Brown and D. Roberts. ammer and interference loca- tion system - design and initial test results. In ION GPS ’99 , 1999.
[2] B.W. O’Hanlon R.H. Mitch, M.L. Psiaki and S.P. Powell. Signal character
istics of civil gps jammers. In ION GNSS 2012 , 2012.
[3] The economist. Gps jamming: No jam tomorrow, January 2013.
[4] K. Spingarn. Passive position location estimation using the extended kal
man fifilter. IEEE Transactions on Aerospace and Electronic Systems , AES-
23(4):558–567, 1987.
[5] F. Fletcher, B. Ristic, and D. Musicki. Recursive estimation of emitter loc
ation using tdoa measurements from two uavs. In Information Fusion, 2007
10th International Conference on , 2007.
[6] D. Musicki and W. Koch. Geolocation using tdoa and fdoa measurements. In Information Fusion, 2008 11th International Conference on , 2008.
[7] C. O’Driscoll D. Borio, J. Fortuny-Guasch. Characterization of gnss jam
mers. Coordinates: A monthly magazine on position, navigation, and beyond , May 2013.
[8] B. Eissfeller D. Fontanella, R. Bauernfeind. In-car gnss jammer localization
with a vehicular ad-hoc network. In the 25th International Technical Meeting
of The Satellite Division of the Institute of Navigation (ION GNSS 2012) ,
2012.
[9] S. Morris E. W. Frew, D. A. Lawrence. Coordinated standoff tracking of
moving targets using lyapunov guidance vector fifields. Journal of Guidance
Control and Dynamics , 31(2):290–306, 2008.
[10] Jeong Heon Lee and R.M. Buehrer. Location estimation using differential rss with spatially correlated shadowing. In Global Telecommunications Confer ence, 2009. GLOBECOM 2009. IEEE , pages 1–6, 2009.