-
Notifications
You must be signed in to change notification settings - Fork 1
/
Drone_System.m
146 lines (126 loc) · 4.61 KB
/
Drone_System.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
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
%==========================================================================
% ◎ ドローンシミュレーション
% M系列信号の入出力特性を求める
%==========================================================================
close all;
clear;
addpath('SI_Function');
%--------------------------------------------------------------------------
% ● 状態空間表現の定義
%--------------------------------------------------------------------------
syms Ix Iy Iz m g % 慣性モーメントと質量、重力加速度
load data/drone_ss;
load data/drone_data;
v_syms = [Ix Iy Iz m g];
v_vals = [drone_Ix drone_Iy drone_Iz drone_m drone_g];
%--------------------------------------------------------------------------
% ● 値の代入
%--------------------------------------------------------------------------
for i = 1:numel(v_syms)
A = subs(A, v_syms(i), v_vals(i));
B = subs(B, v_syms(i), v_vals(i));
D = subs(D, v_syms(i), v_vals(i));
end
clear Ix Iy Iz m g;
Ix = v_vals(1);
Iy = v_vals(2);
Iz = v_vals(3);
m = v_vals(4);
g = v_vals(5);
A_val = double(A);
B_val = double(B);
D_val = double(D);
%--------------------------------------------------------------------------
% ● 伝達関数と出力信号の生成
%--------------------------------------------------------------------------
% パラメータ
m = 1; % 入力の数
p = 1; % 出力の数
sample = 1; % サンプリング周波数
n = 2; % 状態変数の次数
k = 100; % データ行列の行数
ts = 0.001; % サンプリング時間
%--------------------------------------------------------------------------
% ● M系列信号生成 v2
%--------------------------------------------------------------------------
mP = 5; % M系列のmP段シフトレジスタ
mk = 2; % 最終段以外にフィードバックさせる信号
x = randi([0 1], mP, 1); % 状態の初期値をランダムに決定
x = [0 1 0 0 1]'; % 状態の初期値を指定（全て0は禁止）
u = zeros(2^mP-1, 1); % 出力変数の定義
for i = 1:2^mP-1 % 信号の生成
x = [xor(x(mk),x(end));x(1:end-1)]; % フィードバック
u(i) = x(1);
end
for i = 1:2^mP - 1 % 2値信号の値を0,1から他の値に変更
if u(i) == 1
u(i)=0.00001;
else
u(i)=-0.00001;
end
end
s_t = 5; % 信号全体の時間[s]
s_tp = 0.001; % 離散信号1個あたりの継続時間[s] / サンプリング時間
s_p = round(s_t/s_tp/numel(u)); % 1個あたりの連続時間信号数
t_data(:,1) = 0:s_tp:s_tp*s_p*numel(u)-s_tp; % 時間信号を作成
u_data = zeros(size(t_data));
for i = 1:numel(u) % 信号を作成
for j = (i-1)*s_p+1:i*s_p
u_data(j) = u(i);
end
end
u_in = timeseries(u_data, t_data, 'name', 'Position'); % 時系列作成
sim('block/drone_nonlinear',s_t)
sim('block/block_ss',s_t)
%--------------------------------------------------------------------------
% ● MOESP,N4SID実行
%--------------------------------------------------------------------------
clear A B C D;
% 離散データ
[moA, moB, moC, moD] = mf_moesp(data_D.data(:,14)', data_D.data(:,7)', k, n); %MOESP
[n4A, n4B, n4C, n4D] = mf_n4sid(data_D.data(:,14)', data_D.data(:,7)', k/2, n); %N4SID
%--------------------------------------------------------------------------
% ● 伝達関数の算出 moesp
%--------------------------------------------------------------------------
[num, den] = ss2tf(moA,moB,moC,moD);
dP_est = tf(num,den,ts);
%データのプロット
dP_est
cP_est = d2c(dP_est,'zoh');
[mo_y,mo_t] = lsim(cP_est, u_data, t_data);
%FIT算出
clear num den;
[num, den] = tfdata(cP_est);
sim('block/drone_nonlinear_compare',s_t);
clear y yh ya;
y = Cdata.data(:, 7);
yh = Cdata.data(:, 23);
ya = mean(Cdata.data(:, 7));
mofit = (1 - (sqrt(sum((y-yh).^2)))/(sqrt(sum((y-ya).^2))) )*100;
pdt_mt = Cdata.time;
pdt_m = Cdata.data(:, 23);
pdt_true = Cdata.data(:, 7);
%--------------------------------------------------------------------------
% ● 伝達関数の算出 n4sid
%--------------------------------------------------------------------------
clear num den;
[num, den] = ss2tf(n4A,n4B,n4C,n4D);
dP_est = tf(num,den,ts);
%データのプロット
dP_est
cP_est = d2c(dP_est,'zoh');
[n4_y,n4_t] = lsim(cP_est, u_data, t_data);
%FIT算出
clear num den;
[num, den] = tfdata(cP_est);
sim('block/drone_nonlinear_compare',s_t);
clear y yh ya;
y = Cdata.data(:, 7);
yh = Cdata.data(:, 23);
ya = mean(Cdata.data(:, 7));
n4fit = (1 - (sqrt(sum ((y-yh).^2)))/(sqrt(sum((y-ya).^2))) )*100;
pdt_nt = Cdata.time;
pdt_n = Cdata.data(:, 23);
mofit
n4fit
plot_data