使用无人机群进行磁性装置定位matlab代码.zip

classdef UAVNetwork < handle properties NUM_UAVS; network_matrix; UAVS; compute_Y; TIME_STEP; NMPC_handle; THREE_DIMENSIONAL; d_safe; v_max; DELAY_SYNC; FOLLOW_TRAJECTORY; SENSING_HORIZON; sim; UAV_handels; end methods function obj = UAVNetwork(N,TIME_STEP,network_matrix,recievers_pos_ode0,X_hat0,S_RLS0,compute_Y,NMPC_handle,d_safe,v_max,MODE,model_params,UAV_trajs) %% % flags obj.DELAY_SYNC = -1; obj.FOLLOW_TRAJECTORY = false; obj.THREE_DIMENSIONAL = false; if nargin > 10 && MODE == "3D" obj.THREE_DIMENSIONAL = true; end % main variables obj.NUM_UAVS = N; obj.network_matrix = network_matrix; obj.compute_Y = compute_Y; obj.UAVS = cell(obj.NUM_UAVS,1); obj.TIME_STEP = TIME_STEP; obj.NMPC_handle = NMPC_handle; obj.d_safe = d_safe; obj.v_max = v_max; obj.SENSING_HORIZON = 2 * d_safe; %% selection matrices and lists for i = 1 : obj.NUM_UAVS UAV_struct = {}; UAV_struct.TRANSMITTER_FOUND = false; UAV_struct.T_REPLANNING = 4; UAV_struct.selection_matrix = []; UAV_struct.selection_list = []; UAV_struct.complementar_selection_matrix = []; UAV_struct.complementar_selection_list = []; if nargin > 11 UAV_struct.model_params = model_params; end for j = 1 : obj.NUM_UAVS if network_matrix(i,j) == 1 ek = eye(j,obj.NUM_UAVS); ek = ek(j,:); UAV_struct.selection_matrix = [UAV_struct.selection_matrix; ek]; UAV_struct.selection_list = [UAV_struct.selection_list j]; else ek = eye(j,obj.NUM_UAVS); ek = ek(j,:); UAV_struct.complementar_selection_matrix = [UAV_struct.complementar_selection_matrix; ek]; UAV_struct.complementar_selection_list = [UAV_struct.complementar_selection_list j]; end end UAV_struct.dispatcher = Dispatcher(length(UAV_struct.selection_list),TIME_STEP,recievers_pos_ode0(UAV_struct.selection_list,:),X_hat0,S_RLS0); % add prev pos and velocity variable to estimate velocity % and acceleration % UAV_struct.prev_neigh_pos = zeros(N,3); % UAV_struct.prev_neigh_vel = zeros(N,3); % UAV_struct.prev_non_neigh_values = zeros(N,3); UAV_struct.TRACKED_OBS = false; if nargin > 12 obj.FOLLOW_TRAJECTORY = true; UAV_struct.trajectory_ref = squeeze(UAV_trajs(i,:,:)); UAV_struct.trajectory_step = 1; end obj.UAVS{i} = UAV_struct; end %% initialize DRLS algorithm for i = 1 : obj.NUM_UAVS obj.UAVS{i}.X_hat = X_hat0; obj.UAVS{i}.S_RLS = S_RLS0; % extract transmitter estimate position X_hat = obj.UAVS{i}.X_hat; M_hat = [X_hat(1) X_hat(2) X_hat(3);X_hat(2) X_hat(4) X_hat(5); X_hat(3) X_hat(5) X_hat(6)]; % old_transmitter_pos_hat = transmitter_pos_hat; obj.UAVS{i}.transmitter_pos_hat = (inv(M_hat)*X_hat(7:9).').'; % new transmitter estimate obj.UAVS{i}.last_transmitter_pos_hat = obj.UAVS{i}.transmitter_pos_hat; obj.UAVS{i}.last_trustable_transmitter_pos_hat = obj.UAVS{i}.transmitter_pos_hat; end %% initialize UAV controller for i = 1 : obj.NUM_UAVS %% known uavs temp_mask = (obj.UAVS{i}.selection_list ~= i); num_neigh = length(obj.UAVS{i}.selection_list)-1; if ~isempty(obj.UAVS{i}.selection_matrix) if ~obj.THREE_DIMENSIONAL neigh = obj.UAVS{i}.selection_matrix * [recievers_pos_ode0(:,1:2) zeros(size(recievers_pos_ode0,1),1)]; else neigh = obj.UAVS{i}.selection_matrix * recievers_pos_ode0(:,1:3); end neigh = neigh(temp_mask,:); else neigh = []; end neigh = [neigh;zeros(obj.NUM_UAVS-num_neigh,3)]; neigh = [neigh;zeros(2*obj.NUM_UAVS,3)]; %% sensor % temp_mask1 = (obj.UAVS{i}.complementar_selection_list ~= i); % num_non_neigh = obj.NUM_UAVS-num_neigh-1; % if ~isempty(obj.UAVS{i}.complementar_selection_matrix) % if ~obj.THREE_DIMENSIONAL % non_neigh = obj.UAVS{i}.complementar_selection_matrix * [recievers_pos_ode0(:,1:2) zeros(size(recievers_pos_ode0,1),1)]; % else % non_neigh = obj.UAVS{i}.complementar_selection_matrix * recievers_pos_ode0(:,1:3); % end % non_neigh = non_neigh(temp_mask1,:); % else % non_neigh = []; % end % non_neigh = [non_neigh;zeros(obj.NUM_UAVS-num_non_neigh,3)]; if ~obj.THREE_DIMENSIONAL [nearest_obs,min_dist,min_index] = UAVNetwork.compute_nearest_obs([recievers_pos_ode0(i,1:2) 0],[recievers_pos_ode0(1:size(recievers_pos_ode0,1) ~= i,1:2) zeros(size(recievers_pos_ode0,1)-1,1)]); non_neigh = [nearest_obs;zeros(2,3)]; else [nearest_obs,min_dist,min_index] = UAVNetwork.compute_nearest_obs(recievers_pos_ode0(i,1:3),recievers_pos_ode0(1:size(recievers_pos_ode0,1) ~= i,1:3)); non_neigh = [nearest_obs;zeros(2,3)]; end num_non_neigh = 3; if obj.UAVS{i}.TRACKED_OBS && min_dist >= obj.SENSING_HORIZON num_non_neigh = -1; non_neigh = zeros(3,3); obj.UAVS{i}.TRACKED_OBS = false; elseif ~obj.UAVS{i}.TRACKED_OBS && min_dist <= obj.SENSING_HORIZON obj.UAVS{i}.TRACKED_OBS = min_index; elseif ~obj.UAVS{i}.TRACKED_OBS && min_dist >= obj.SENSING_HORIZON num_non_neigh = -1; non_neigh = zeros(3,3); elseif obj.UAVS{i}.TRACKED_OBS && min_dist <= obj.SENSING_HORIZON if min_index == obj.UAVS{i}.TRACKED_OBS % non_neigh(2,:) = (non_neigh(1,:)-prev_non_neigh_matrix(1,:))/obj.NMPC_handle.PredictionHorizon; % non_neigh(3,:) = (non_neigh(2,:)-prev_non_neigh_matrix(2,:))/obj.NMPC_handle.PredictionHorizon; else obj.UAVS{i}.TRACKED_OBS = min_index; end end %% X0 = recievers_pos_ode0(i,:); obj.UAVS{i}.lambda_factor = 0.1; % promote robust responces if ~obj.THREE_DIMENSIONAL curr_ref = EMA_const_reference(X0,[obj.UAVS{i}.transmitter_pos_hat(1:2) 0 0],obj.UAVS{i}.lambda_factor,NMPC_handle.PredictionHorizon); U0 = [0 0]; NMPC_params = {neigh,num_neigh,non_neigh,num_non_neigh,d_safe,v_max}; else