卡尔曼滤波KF、扩展EKF、无际UKF、容积CKF以及交互式多模型算法IMM Matlab GUI界面设计 动画

function [xV] = app_fun_track_n(X,Track_text_n,T,n_uav) % 备注： 轨迹修改为下面fun_track函数，T采样时间（不用变）， % 出了修改轨迹之外，记得修改输入x（初始状态），N总时刻 % 转弯模型，还要修改转弯角速度，即w1和w2 % 记得还要修改节点布局，不然轨迹很远，节点拓扑很小，定位不上 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% 目标运动轨迹产生 %% 解析track_text %模型2:CV模型 Fk_cv=[1 T 0 0; 0 1 0 0; 0 0 1 T; 0 0 0 1;]; q2=0.03; % 目标运动学标准差，过程噪声 Qk_cv=q2^2*[T^3/3, T^2/2, 0, 0; T^2/2, T^2, 0, 0; 0, 0, T^3/3, T^2/2; 0, 0, T^2/2, T^2 ;];% 过程噪声协方差 Gk_cv=eye(4); X_init=X; Track_text = regexp(Track_text_n,'\ \|\ ','split'); for num=1:n_uav X=X_init; splitStr = regexp(Track_text(num),'\ ','split'); t_start=0; t_end=0; speed=0; speed_last=sqrt(X(2)^2+X(4)^2); N=length(splitStr); for i=1:N split_split_Str = regexp(splitStr(i),'\&','split'); % 提取时间 t_end=str2double(split_split_Str(1)); % 分离速度和旋转角速度 split_split_split_Str = regexp(split_split_Str(2),'\|','split'); % 提取运行速度 speed=str2double(split_split_split_Str(1)); factor=speed*1.0/speed_last; X(2)=X(2)*factor; X(4)=X(4)*factor; % 提取旋转角速度 num_fraction_w1=regexp(split_split_split_Str(2), '\d+', 'match'); num_decimal_w1=regexp(split_split_split_Str(2), '\d+[.]\d+', 'match'); num_pi=regexp(split_split_split_Str(2), 'pi', 'match','ignorecase'); if length(num_decimal_w1)==1 xiaoshu=str2double(num_decimal_w1(1)); w1=1.0*xiaoshu; elseif length(num_fraction_w1)==2 % 分数 fenzi=str2double(num_fraction_w1(1)); fenmu=str2double(num_fraction_w1(2)); w1=1.0*fenzi/fenmu; else % 整数 zhengshu=str2double(num_fraction_w1(1)); w1=1.0*zhengshu; end if ~isempty(num_pi) w1=w1*pi; end temp=char(split_split_split_Str(2)); if isequal(temp(1),"-") w1=w1*-1; end n=length(X);% 状态维数 %% 轨迹分阶段生成 if isequal(split_split_Str(3),'CT')||isequal(split_split_Str(3),'ct') %% target model % 模型1：CT模型参数 Fk_ct1=[1 sin(w1*T)/w1 0 -(1-cos(w1*T))/w1 ; 0 cos(w1*T) 0 -sin(w1*T) ; 0 (1-cos(w1*T))/w1 1 sin(w1*T)/w1 ; 0 sin(w1*T) 0 cos(w1*T) ;]; % 转弯模型 q1=0.03; % 目标运动学标准差，过程噪声 Qk_ct1= q1^2*[2*(w1*T-sin(w1*T))/w1^3 (1-cos(w1*T))/w1^2 0 (w1*T-sin(w1*T))/w1^2 ; (1-cos(w1*T))/w1^2 T -(w1*T-sin(w1*T))/w1^2 0 ; 0 -(w1*T-sin(w1*T))/w1^2 2*(w1*T-sin(w1*T))/w1^3 (1-cos(w1*T))/w1^2 ; (w1*T-sin(w1*T))/w1^2 0 (1-cos(w1*T))/w1^2 T;]; Gk_ct1=eye(4); for k=t_start+1:t_end X = Fk_ct1*X + Gk_ct1*sqrtm(Qk_ct1)*randn(n,1); %CT % X = Fk_ct2*X; %CT xV(:,k,num) = X; end elseif isequal(split_split_Str(3),'CV')||isequal(split_split_Str(3),'cv') for k=t_start+1:t_end X = Fk_cv*X + Gk_cv*sqrtm(Qk_cv)*randn(n,1); %CV， 轨迹生成加过程噪声 % % X = Fk_cv*X ; %CV， 轨迹生成不加过程噪声 xV(:,k,num)=X; end end t_start=t_end; speed_last=speed; end end