-
Notifications
You must be signed in to change notification settings - Fork 8
/
ex_car_wrong.m
executable file
·114 lines (90 loc) · 2.42 KB
/
ex_car_wrong.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
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
clear all,close all,clc
addpath('./tools');
addpath('./data');
addpath('./SLR_dev');
%% load Data
basis_function.work='off';
data=load('car2.mat');
start = 1;
num = 369;
index = start:num;
flag = data.flag(index);
du = data.dy(index);
v = data.v(index)/10;
%% library
polyorder = 4;
memory = 4;
A= library(v,polyorder,memory,basis_function);
v_k1 = v(memory+1:end-1,:);
v_k2 = v(memory:end-2,:);
v_k3 = v(memory-1:end-3,:);
v_k4 = v(memory-2:end-4,:);
A = A(memory+2:end,:);
du = du(memory+2:end);
v = v(memory+2:end);
flag = flag(memory+2:end);
%% identify subsystem
parameter.lambda = [1e-1 0.00001]; % the lambda of z in algorithm 1.
parameter.MAXITER = 5;
parameter.max_s = 20;%the max s
parameter.epsilon = [100 8];
parameter.Phi = A;
parameter.y = du;
parameter.normalize_y = 1;
[result]=ihyde(parameter);
result.epsilon = parameter.epsilon(2);
result.lambda = parameter.lambda(2);
result.threshold = [0.05];
final_result = finetuning( result);
sys = final_result.sys;
idx_sys = final_result.idx;
%% identify logic
Phi2 = [ones(size(flag)) flag sin(v) cos(v) tan(v) (v_k1-v_k4)./v_k2 v_k1.*tan(v_k3)];
para_log.idx_sys = idx_sys;
para_log.beta = 0.1;
para_log.y = du;
para_log.Phi2 = Phi2;
para_log.normalize = 1;
[syslogic,labelMat,data] = ihydelogic(para_log);
%% compare with the right answer
judge = 7*ones(size(flag));
judge(idx_sys{1}) = 0;
judge(idx_sys{2}) = 1;
wrong_position = find((judge-flag)~=0);
ans_sys_idx{1} = find(flag==1);
ans_sys_idx{2} = find(flag==0);
%%
close all
figure(1)
axes1 = axes('Parent',figure(1));
hold on
color = {'r' ,'b'};
for i =1:2
input1 = zeros(size(v));
input1(ans_sys_idx{i},1) = v(ans_sys_idx{i},1);
input1(input1==0)=nan;
plot(input1(:,1),'Color',color{i},'LineWidth',3);
end
legend('Subsystem_1','Subsystem_2')
xlabel('Time','FontWeight','bold');
ylabel('Velocity','FontWeight','bold');
box(axes1,'on');
set(axes1,'FontSize',14,'FontWeight','bold','LineWidth',1.5);
legend(axes1,'show');
%%
figure(2)
axes1 = axes('Parent',figure(2));
hold on
color = {'r' ,'b'};
for i =1:2
input1 = zeros(size(du));
input1(ans_sys_idx{i},1) = du(ans_sys_idx{i},1);
input1(input1==0)=nan;
plot(input1(:,1),'Color',color{i},'LineWidth',3);
end
legend('Subsystem_1','Subsystem_2')
xlabel('Time','FontWeight','bold');
ylabel('dpwm','FontWeight','bold');
box(axes1,'on');
set(axes1,'FontSize',14,'FontWeight','bold','LineWidth',1.5);
legend(axes1,'show');