-
Notifications
You must be signed in to change notification settings - Fork 3
/
Copy pathtest_PCA_for_DF4.m
83 lines (82 loc) · 2.19 KB
/
test_PCA_for_DF4.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
clear all;
close all;
addpath(genpath(pwd))
% function callqpact
%=====================B of DF4==========================================
B=[-0.5 0 0.5 0;
0 -0.5 0 0.5;
0.25 0.25 0.25 0.25];
umin=[1;1;1;1]*(-20)*pi/180;
umax=[1;1;1;1]*20*pi/180;
%===================================幅值测试==================================================
N=40;
[XX,YY,ZZ] = sphere(N);
%==================================速度约束测试==================================
t=0:0.01:0.5;
% X1=0.3*sin(pi*t);Y1=0.3*cos(pi*t);Z1=0.1*sin(pi*t);
% X2=0.1*sin(pi*t);Y2=0.1*cos(pi*t);Z2=0.05*sin(pi*t);
X1=0.0;Y1=0.0;Z1=0.3;
X2=0.1;Y2=0.3;Z2=0;
X=X1+X2;Y=Y1+Y2;Z=Z1+Z2;
x=zeros(4,length(X1));%(N+1)^2);
xx=zeros(4,length(X1));%(N+1)^2);
x1=zeros(4,(N+1)^2);
x2=zeros(4,(N+1)^2);
x3=zeros(4,(N+1)^2);
x4=zeros(4,(N+1)^2);
iter=zeros((N+1)^2,1);
fval=zeros((N+1)^2,1);
t=zeros(3,(N+1)^2);
u=[0;0;0;0];
uu=zeros(4,1);
u1=[0;0;0;0];
u2=[0;0;0;0];
u3=[0;0;0;0];
u4=[0;0;0;0];
p_limits=20;
for i=1:(N+1)^2%
vv=1*[XX(i);YY(i);ZZ(i)];
% [u,z,iters] = allocator_dir_LPwrap_4(B, v, umin,umax)
uu = allocator_dir_LPwrap_4(B,vv, umin,umax);
xx(:,i)=uu;
end
for i=1:length(X1) %(N+1)^2%
v1=1*[X1(i);Y1(i);Z1(i)]; % 虚拟指令
v2=1*[X2(i);Y2(i);Z2(i)];
v=1*[X(i);Y(i);Z(i)];
% % %==================有效集=====================
[u,~,~] = allocator_dir_LPwrap_4(B,v, umin,umax);
x(:,i)=u;
[u1,~,~] = allocator_dir_LPwrap_4(B,v1, umin,umax);
x1(:,i)=u1;
[u2,~,~] = allocator_dir_LPwrap_4(B,v2, umin,umax);
x2(:,i)=u2;
% 改进
% u3=two_dir_alloc_df4(B,v1, v2, umin,umax);
u3=prioritized_control_allocator(v1,v2,B,umin,umax,100);
x3(:,i)=u3;
% u4=wls_ca_4df_pv_limit(v, u4, p_limits, 1);
% x4(:,i)=u4;
end
UU=B*xx;
U=B*x;
U1=B*x1;
U2=B*x2;
U3=B*x3;
U4=B*x4;
figure,
plot3(UU(1,:),UU(2,:),UU(3,:),'k.');grid on;
hold on;
plot3(X,Y,Z,'b>');
hold on;
plot3(U(1,:),U(2,:),U(3,:),'g>');grid on;
hold on;
plot3(U1(1,:),U1(2,:),U1(3,:),'r*');grid on;
% legend('边界','给定','原','改进')
hold on;
plot3(U2(1,:),U2(2,:),U2(3,:),'g+');grid on;
hold on;
plot3(U3(1,:),U3(2,:),U3(3,:),'b*');grid on;
plot3(U4(1,:),U4(2,:),U4(3,:),'r+');grid on;
hold on;
legend('边界','给定','直接分配总和','扰动','动态','改进','二次')