Bode Plot Index Exceeds Array Elements Error

2024 3 20
0 个回答
更新时间：2024 3 21
10 次查看（30 天）

0 个投票

Error using DynamicSystem/bode
Index exceeds the number of array elements. Index
must not exceed 7.
Error in bf_controller_tuning (line 496)
bode(ax(1), P / Gf_ana, 'k', omega_bode, opt), title('Plant P')
Hi All - I am on a mac, and am getting this error when running a script. The interesting thing is that a PC user runs the same and gets no error. Now that could be for some other buried reason like he has the right add-ons but we checked and I do also. I'm running a trial and have 5 days left to get this script running...!

5 个评论
显示 3更早的评论 隐藏 3更早的评论

Stephen
Stephen 2024-3-20
Here is the full code block:
%% plant and used controllers
figure(expand_multiple_figure_nr(4, multp_fig_nr))
ax(1) = subplot('Position', pos_bode(1,:));
opt.YLim = {[1e-4 1e2], [-180 180]}; opt.MagScale = 'log';
bode(ax(1), P / Gf_ana, 'k', omega_bode, opt), title('Plant P')
hold off, grid on
ax(2) = subplot('Position', pos_bode(2,:));
opt.YLimMode = {'auto'}; opt.MagScale = 'linear';
bodemag(ax(2), C_T * C_Guw, 'k', omega_bode, opt), title(''), ylabel('Coherence')
linkaxes(ax, 'x'), clear ax
set(findall(gcf, 'type', 'line'), 'linewidth', linewidth)
compare analytical to estimated controllers
figure(expand_multiple_figure_nr(8, multp_fig_nr))
opt.YLim = {[1e-2 1e2], [-180 180]}; opt.MagScale = 'log';
bode(Cpi, Cd, Cpi_ana, Cd_ana, omega_bode, opt), title('Cpi, Cd')
set(findall(gcf, 'type', 'line'), 'linewidth', linewidth)
Paul
Paul 2024-3-20
This line:
bode(ax(1), P / Gf_ana, 'k', omega_bode, opt)
is not valid based on the doc page bode or bode. Using ax(1) as the first argument seems to work (though not documented AFAICT), but I don't see anything about an opt argument, which causes an error, though not the same error reported above.
It would be helpful to have all of information needed to recreate the error messsage.
Michael Ernst
Michael Ernst 2024-3-20
The full script file can be found in my repo here: https://github.com/pichim/bf_controller_tuning
The script uses a custom function libary that can be found here: https://github.com/pichim/bf_function_libary
bf_controller_tuning.m was rigorously tested on Matlab R2023a on different windows machines. so it might have something to do with
  • Mac
  • Newer or older Matlab versions
  • Matlab trial version
I suspect Mac, but i have no Mac available to test and debug the issue. so if anyone could test this, it would be greatly apprechiated!
Stephen
Stephen 2024-3-20
编辑：Stephen 2024-3-20
Thanks for the responses...this is the whole script. The error comes at line 496 which is given above....
clc, clear variables
% addpath ../../bf_function_libary/
addpath ../bf_function_libary/
%%
% [fList, pList] = matlab.codetools.requiredFilesAndProducts('bf_controller_tuning.m')
% 165+49+49 = 263 CHF
% 263*1.05 = 276 EUR
% TODO:
% - the iterm_relax parameter can be used to decide if we need to
% compensate it's effects
% - create something like para.blackbox_high_resolution in blackbox, so
% that it can automatically be evaluated if it was a chirp excitaion
% - check out "flightModeFlags" for sinarg evaluation
% - figure out motor_magic_offset <- can't remember atm
% startup('abs', 'Hz');
% choose an axis: 1: roll, 2: pitch, 3: yaw
ind_ax = 1;
% parameters
do_compensate_iterm = false;
do_show_dev_figures = false;
do_show_spec_figures = true;
do_show_motor_figures = false;
do_show_motor_p_cntrl = false; % has only an effect if do_show_motor_figures = true
do_insert_legends = false;
multp_fig_nr = ind_ax;
linewidth = 1.2;
set(0, 'defaultAxesColorOrder', get_my_colors);
pos_bode = [0.1514, 0.5838-0.2, 0.7536, 0.3472+0.2; ... % this is a bit hacky
0.1514, 0.1100 , 0.7536, 0.1917 ];
% bodeoptions (whatever you like)
% opt = bodeoptions('cstprefs');
opt = bodeoptions;
opt.FreqUnits = 'Hz';
opt.MagUnits = 'abs';
opt.MagScale = 'log';
opt.PhaseWrapping = 'on';
opt.Grid = 'on';
% define quad and build path to *.bbl.csv file
flight_folder = '20240225';
quad = 'apex5';
% path = ['/03_', quad, '/01_blackbox_logs/', ...
% flight_folder, '/', flight_folder, '_', quad, '_00.bbl.csv'];
path = '20240225_apex5_00.bbl.csv';
% file_path = ['../../../00_quads', path];
file_path = path;
% extract header information
[para, Nheader, ind] = extract_header_information(file_path);
% read the data
tic
try
load([file_path(1:end-8), '.mat'])
catch exception
data = readmatrix(file_path, 'NumHeaderLines', Nheader);
% import_data = importdata(file_path, ',', Nheader);
% data = import_data.data;
save([file_path(1:end-8), '.mat'], 'data');
end
[Ndata, Nsig] = size(data) %#ok
toc
% expand index
ind.axisSumPI = ind.axisError(end) + (1:3);
ind.sinarg = ind.debug(1);
% convert and evaluate time
time = (data(:,ind.time) - data(1,ind.time)) * 1.0e-6;
dtime_meas_mus = diff(time) * 1.0e6;
figure(99)
plot(time(1:end-1), dtime_meas_mus), grid on
title(sprintf('Mean: %0.2f mus, Median: %0.2f mus, Std: %0.2f mus\n', ...
mean(dtime_meas_mus), ...
median(dtime_meas_mus), ...
std(dtime_meas_mus)))
xlabel('Time (sec)'), ylabel('Ts log (mus)')
xlim([0, time(end)])
set(findall(gcf, 'type', 'line'), 'linewidth', linewidth)
% unscale highResolutionGain
if para.blackbox_high_resolution
blackbox_high_resolution_scale = 10.0;
ind_bb_high_res = [ind.gyroADC, ind.gyroUnfilt, ind.rcCommand, ind.setpoint(1:3)];
data(:, ind_bb_high_res) = 1.0 / blackbox_high_resolution_scale * data(:, ind_bb_high_res);
end
% unscale and remap sinarg
sinargScale = 5.0e3;
data(:,ind.sinarg) = 1.0 / sinargScale * data(:,ind.sinarg);
% assign negative sign for pid error
data(:,ind.axisError) = -data(:,ind.axisError);
% create an additional entry for the pi sum
data = [data, data(:,ind.axisP) + data(:,ind.axisI)];
% create different sampling times
Ts = para.looptime * 1.0e-6; % gyro
Ts_cntr = para.pid_process_denom * Ts; % cntrl
Ts_log = para.frameIntervalPDenom * Ts_cntr; % logging
% get evaluation index
ind_eval = get_ind_eval(data(:,ind.sinarg), data(:,ind.gyroADC(ind_ax)));
data(~ind_eval,ind.sinarg) = 0.0;
T_eval_tot = size(data(ind_eval,ind.sinarg), 1) * Ts_log %#ok
% calculate average throttle
throttle_avg = median(data(ind_eval,ind.setpoint(4))) / 1.0e3;
%% show gyro to select Teval and spectras (gyro and pid sum)
figure(1)
ax(1) = subplot(311);
plot(ax(1), time, data(:,[ind.setpoint(1), ind.gyroUnfilt(1), ind.gyroADC(1)])), grid on, ylabel('Roll (deg/sec)')
title('Gyro Signals')
if do_insert_legends, legend('setpoint', 'gyro', 'gyroADC', 'location', 'best'), end %#ok
ax(2) = subplot(312);
plot(ax(2), time, data(:,[ind.setpoint(2), ind.gyroUnfilt(2), ind.gyroADC(2)])), grid on, ylabel('Pitch (deg/sec)')
ax(3) = subplot(313);
plot(ax(3), time, data(:,[ind.setpoint(3), ind.gyroUnfilt(3), ind.gyroADC(3)])), grid on, ylabel('Yaw (deg/sec)'), xlabel('Time (sec)')
linkaxes(ax, 'x'), clear ax, xlim([0, time(end)])
set(findall(gcf, 'type', 'line'), 'linewidth', linewidth)
% select data for spectras
data_for_spectras = data(:, [ind.gyroUnfilt, ...
ind.gyroADC, ...
ind.axisSum, ...
ind.setpoint(1:3)]);
Nest = round(5.0 / Ts_log);
koverlap = 0.9;
Noverlap = round(koverlap * Nest);
window = hann(Nest);
[pxx, freq] = estimate_spectras(data_for_spectras, window, Noverlap, Nest, Ts_log);
spectras = sqrt(pxx); % power -> amplitude (dc needs to be scaled differently)
figure(2)
ax(1) = subplot(211);
plot(ax(1), freq, spectras(:, 1:6)), grid on, ylabel('Gyro (deg/sec)'), set(gca, 'YScale', 'log')
title('Power Spectras')
if do_insert_legends, legend('gyro Roll', 'gyro Pitch', 'gyro Yaw', 'gyroADC Roll', 'gyroADC Pitch', 'gyroADC Yaw', 'location', 'best'), end %#ok
ax(2) = subplot(212);
plot(ax(2), freq, spectras(:, 7:9)), grid on, ylabel('AxisSum'), xlabel('Frequency (Hz)'), set(gca, 'YScale', 'log')
if do_insert_legends, legend('axisSum Roll', 'axisSum Pitch', 'axisSum Yaw', 'location', 'best'), end %#ok
linkaxes(ax), clear ax, axis([0 1/2/Ts_log 1e-3 1e1])
set(findall(gcf, 'type', 'line'), 'linewidth', linewidth)
%%
% spectrogram
if (do_show_spec_figures)
% parameters
Nest = round(0.2 / Ts_log);
koverlap = 0.9;
Noverlap = round(koverlap * Nest);
window = hann(Nest);
Nres = 100;
c_lim = [3e-2 3e0];
for spectrogram_nr = 1:3
[pxx, freq, throttle] = estimate_spectrogram(data(:,ind.gyroUnfilt(spectrogram_nr)), ...
data(:,ind.setpoint(4)) / 10.0, ...
window, Noverlap, Nest, Nres, Ts_log);
spectrograms = sqrt(pxx); % power -> amplitude (dc needs to be scaled differently)
figure(22)
subplot(230 + spectrogram_nr)
qmesh = pcolor(freq, throttle, spectrograms);
set(qmesh, 'EdgeColor', 'None', 'FaceColor', 'interp');
xlabel('Frequency (Hz)'), ylabel('Throttle (%)')
% colorbar()
colormap('jet')
set(gca, 'ColorScale', 'log')
clim(c_lim);
ylim([0 100])
end
for spectrogram_nr = 1:3
[pxx, freq, throttle] = estimate_spectrogram(data(:,ind.gyroADC(spectrogram_nr)), ...
data(:,ind.setpoint(4)) / 10.0, ...
window, Noverlap, Nest, Nres, Ts_log);
spectrograms = sqrt(pxx); % power -> amplitude (dc needs to be scaled differently)
figure(22)
subplot(230 + spectrogram_nr + 3)
qmesh = pcolor(freq, throttle, spectrograms);
set(qmesh, 'EdgeColor', 'None', 'FaceColor', 'interp');
xlabel('Frequency (Hz)'), ylabel('Throttle (%)')
% colorbar()
colormap('jet')
set(gca, 'ColorScale', 'log')
clim(c_lim);
ylim([0 100])
end
end
%%
if (do_show_dev_figures)
figure(222) %#ok
ax(1) = subplot(211);
plot(ax(1), time, data(:,ind.setpoint(1:3))), grid on, ylabel('Setpoint (deg/sec)')
title('Chirp Excitation Signals')
if do_insert_legends, legend('setpoint Roll', 'setpoint Pitch', 'setpoint Yaw', 'location', 'best'), end %#ok
ax(2) = subplot(212);
plot(ax(2), time, data(:,ind.sinarg)), grid on, ylabel('Sinarg (rad)'), xlabel('Time (sec)')
linkaxes(ax, 'x'), clear ax, xlim([0, time(end)])
set(findall(gcf, 'type', 'line'), 'linewidth', linewidth)
end
figure(3)
ax(1) = subplot(411);
plot(ax(1), time, data(:,ind.gyroUnfilt)), grid on, ylabel('Gyro (deg/sec)')
ax(2) = subplot(412);
plot(ax(2), time, data(:,ind.axisSum)), grid on, ylabel('AxisSum')
ax(3) = subplot(413);
plot(ax(3), time, data(:,ind.motor)), grid on, ylabel('Motor')
ax(4) = subplot(414);
plot(ax(4), time, data(:,ind.setpoint(4))), grid on, ylabel('Throttle'), xlabel('Time (sec)')
linkaxes(ax, 'x'), clear ax, xlim([0, time(end)])
set(findall(gcf, 'type', 'line'), 'linewidth', linewidth)
%% frequency response estimation and calculation
Nest = round(2.5 / Ts_log);
koverlap = 0.9;
Noverlap = round(koverlap * Nest);
window = hann(Nest);
% rotating filter
Dlp = sqrt(3) / 2;
wlp = 2 * pi * 10;
Glp = c2d(tf(wlp^2, [1 2*Dlp*wlp wlp^2]), Ts_log, 'tustin');
% T , Gyw: w -> y
inp = apply_rotfiltfilt(Glp, data(:,ind.sinarg), data(:,ind.setpoint(ind_ax)));
out = apply_rotfiltfilt(Glp, data(:,ind.sinarg), data(:,ind.gyroADC(ind_ax)) );
[T, C_T] = estimate_frequency_response(inp(ind_eval), out(ind_eval), window, Noverlap, Nest, Ts_log);
% SCw, Guw: w -> u
out = apply_rotfiltfilt(Glp, data(:,ind.sinarg), data(:,ind.axisSum(ind_ax)));
[Guw, C_Guw] = estimate_frequency_response(inp(ind_eval), out(ind_eval), window, Noverlap, Nest, Ts_log);
% Gvw: w -> v (v := u only from PI cntrl)
out = apply_rotfiltfilt(Glp, data(:,ind.sinarg), data(:,ind.axisSumPI(ind_ax)));
[Gvw, C_Gvw] = estimate_frequency_response(inp(ind_eval), out(ind_eval), window, Noverlap, Nest, Ts_log);
% P , Gyu: u -> y
P = T / Guw;
% % P , Gyu: u -> y (direct measurement, results are slightly worse)
% inp = apply_rotfiltfilt(Glp, data(:,ind.sinarg), data(:,ind.axisSum(ind_ax)));
% out = apply_rotfiltfilt(Glp, data(:,ind.sinarg), data(:,ind.gyroADC(ind_ax)));
% [Pd, C_Pd] = estimate_frequency_response(inp(ind_eval), out(ind_eval), window, Noverlap, Nest, Ts_log);
% controller frf estimates
Cpi = Gvw / (1 - T);
Cd = Guw * Gvw / T * (1 / Guw - 1 / Gvw);
% index and frequency for bode plots
ind_freq = P.Frequency <= 1/2/Ts_log;
omega_bode = 2*pi*P.Frequency(ind_freq);
if (do_show_dev_figures)
figure(30) %#ok
ax(1) = subplot('Position', pos_bode(1,:));
opt.YLimMode = {'auto'}; opt.MagScale = 'log';
bode(ax(1), T, 'b', omega_bode, opt), title('Tracking T')
ax(2) = subplot('Position', pos_bode(2,:));
opt.MagScale = 'linear';
bodemag(ax(2), C_T, 'b', omega_bode, opt), title(''), ylabel('Coherence')
linkaxes(ax, 'x'), clear ax
set(findall(gcf, 'type', 'line'), 'linewidth', linewidth)
figure(31)
ax(1) = subplot('Position', pos_bode(1,:));
opt.YLimMode = {'auto'}; opt.MagScale = 'log';
bode(ax(1), Guw, 'g', omega_bode, opt), title('Guw')
ax(2) = subplot('Position', pos_bode(2,:));
opt.MagScale = 'linear';
bodemag(ax(2), C_Guw, 'g', omega_bode, opt), grid on, title(''), ylabel('Coherence')
linkaxes(ax, 'x'), clear ax
set(findall(gcf, 'type', 'line'), 'linewidth', linewidth)
figure(32)
ax(1) = subplot('Position', pos_bode(1,:));
opt.YLimMode = {'auto'}; opt.MagScale = 'log';
bode(ax(1), Gvw, 'r', omega_bode, opt), title('Gvw')
ax(2) = subplot('Position', pos_bode(2,:));
opt.MagScale = 'linear';
bodemag(ax(2), C_Gvw, 'r', omega_bode, opt), title(''), ylabel('Coherence')
linkaxes(ax, 'x'), clear ax
set(findall(gcf, 'type', 'line'), 'linewidth', linewidth)
end
%%
% clear motors
% P_motor = 50;
%
% M_mod = c2d(ss(tf(0.09, [1/(2*pi*8) 1])), Ts_log);
% Tt_M = (50) / (2*pi*100) * pi/180;
% M_mod.InputDelay = round(Tt_M / Ts_log);
%
% damp(feedback(M_mod * P_motor, 1))
%%
if do_show_motor_figures
P_motor = 0.1;
clear motors
for motor_nr = 1:4
motor_str = num2str(motor_nr);
motor_magic_offset = 2.4;
inp = data(:,ind.motor(motor_nr)) * 100 / 2000 - motor_magic_offset;
out = data(:,ind.eRPM(motor_nr)) * 200 / (para.motor_poles * 60);
figure(300)
subplot(220 + motor_nr)
hist3([out, inp], [100 100], ...
'EdgeColor', 'interp', 'CDataMode', 'auto')
title(['Motor ', motor_str])
xlabel('RPS (Hz)'), ylabel('Motor (%)')
% colorbar
colormap('jet')
set(gca, 'ColorScale', 'log')
view(2)
ylim([0 100])
% Nest = round(0.2 / Ts_log);
% koverlap = 0.9;
% Noverlap = round(koverlap * Nest);
% window = hann(Nest);
% Nres = 100;
%
% [pxx, freq, motor] = estimate_spectrogram(data(:,ind.gyroUnfilt(spectrogram_nr)), ...
% inp, ...
% window, Noverlap, Nest, Nres, Ts_log);
% spectrograms = sqrt(pxx); % power -> amplitude (dc needs to be scaled differently)
%
% figure(310)
% subplot(220 + motor_nr)
% qmesh = pcolor(freq, motor, spectrograms);
% set(qmesh, 'EdgeColor', 'None', 'FaceColor', 'interp');
% title(['Motor ', motor_str])
% xlabel('Frequency (Hz)'), ylabel('Motor (%)')
% % colorbar()
% colormap('jet')
% set(gca, 'ColorScale', 'log')
% clim([3e-2 3e0]);
% ylim([0 100])
Nest = round(2.5 / Ts_log);
koverlap = 0.9;
Noverlap = round(koverlap * Nest);
window = hann(Nest);
% Pm
inp = apply_rotfiltfilt(Glp, data(:,ind.sinarg), inp);
out = apply_rotfiltfilt(Glp, data(:,ind.sinarg), out);
% out = apply_rotfiltfilt(Glp, data(:,ind.sinarg), data(:,ind.eRPM(motor_nr) ) / para.motor_poles);
[M, C_M] = estimate_frequency_response(inp(ind_eval), out(ind_eval), window, Noverlap, Nest, Ts_log);
motors.(['M_' , motor_str]) = M;
motors.(['C_M_', motor_str]) = C_M;
if (do_show_motor_p_cntrl)
Lm = M*P_motor; %#ok
Sm = 1.0 / (1.0 + Lm);
Tm = Lm*Sm;
SPm = M*Sm;
SCm = P_motor*Sm;
motors.(['S_' , motor_str]) = Sm;
motors.(['T_' , motor_str]) = Tm;
motors.(['SP_' , motor_str]) = SPm;
motors.(['SC_' , motor_str]) = SCm;
end
end
figure(301)
ax(1) = subplot('Position', pos_bode(1,:));
opt.YLim = {[1e-2 1e2], [-180 180]}; opt.MagScale = 'log';
bode(ax(1), motors.M_1, motors.M_2, motors.M_3, motors.M_4, omega_bode, opt), grid on
title('Motor Pm')
ax(2) = subplot('Position', pos_bode(2,:));
opt.YLimMode = {'auto'}; opt.MagScale = 'linear';
bodemag(ax(2), motors.C_M_1, motors.C_M_2, motors.C_M_3, motors.C_M_4, omega_bode, opt), ylabel('Coherence')
title('')
if do_insert_legends, legend('Motor 1', 'Motor 2', 'Motor 3', 'Motor 4', 'location', 'best'), end %#ok
linkaxes(ax, 'x'), clear ax
set(findall(gcf, 'type', 'line'), 'linewidth', linewidth)
if (do_show_motor_p_cntrl)
figure(302)
ax(1) = subplot(221);
opt.YLim = {[1e-3 1e1], [-180 180]};
opt.MagScale = 'log';
bode(ax(1), motors.T_1, motors.T_2, motors.T_3, motors.T_4, omega_bode, opt), title('Tracking T' )
ax(2) = subplot(222);
bode(ax(2), motors.S_1, motors.S_2, motors.S_3, motors.S_4, omega_bode, opt), title('Sensitivity S' )
ax(3) = subplot(223);
opt.YLim = {[1e-2 1e1], [-180 180]};
bode(ax(3), motors.SC_1, motors.SC_2, motors.SC_3, motors.SC_4, omega_bode, opt), title('Controller Effort SC')
ax(4) = subplot(224);
opt.YLim = {[1e-2 1e+1], [-180 180]};
bode(ax(4), motors.SP_1, motors.SP_2, motors.SP_3, motors.SP_4, omega_bode, opt), title('Compliance SP')
set(findall(gcf, 'type', 'line'), 'linewidth', linewidth)
% step responses
f_max = 300; % needs to be set according to the coherence, if inf then it has no effect
step_time = (0:Nest-1).'*Ts_log;
step_resp = [calculate_step_response_from_frd(motors.M_1, f_max), ...
calculate_step_response_from_frd(motors.M_2, f_max), ...
calculate_step_response_from_frd(motors.M_3, f_max), ...
calculate_step_response_from_frd(motors.M_4, f_max), ...
calculate_step_response_from_frd(motors.T_1, f_max), ...
calculate_step_response_from_frd(motors.T_2, f_max), ...
calculate_step_response_from_frd(motors.T_3, f_max), ...
calculate_step_response_from_frd(motors.T_4, f_max)];
figure(303)
ax(1) = subplot(211);
plot(ax(1), step_time, step_resp(:,1:4)), grid on
title('Motor'), ylabel('RPS (Hz)')
if do_insert_legends, legend('Motor 1', 'Motor 2', 'Motor 3', 'Motor 4', 'location', 'best'), end %#ok
% ylim([0 0.1])
ax(2) = subplot(212);
plot(ax(2), step_time, step_resp(:,5:8)), grid on
title('Tracking T'), ylabel('RPM (Hz)')
% ylim([0 1.0])
linkaxes(ax, 'x'), clear ax, xlim([0 0.5])
set(findall(gcf, 'type', 'line'), 'linewidth', linewidth)
end
end
%% downsample analytical controller transferfunction and convert to frd objects
[Cpi_ana, Cd_ana, Gf_ana, PID, para_used] = ...
calculate_transfer_functions(para, ind_ax, throttle_avg, Ts_cntr);
% downsample analytical controller transferfunction and convert to frd objects
if Gf_ana.Ts < Ts_log % by using Gf_ana.Ts we secure that we do this only once
Gf_ana = downsample_frd(Gf_ana , Ts_log, P.Frequency);
Cpi_ana = downsample_frd(Cpi_ana, Ts_log, P.Frequency);
Cd_ana = downsample_frd(Cd_ana , Ts_log, P.Frequency);
end
%% plant and used controllers
% % figure(expand_multiple_figure_nr(4, multp_fig_nr))
% ax(1) = subplot('Position', pos_bode(1,:));
% opt.YLim = {[1e-4 1e2], [-180 180]}; opt.MagScale = 'log';
% bode(ax(1), P / Gf_ana, 'k', omega_bode, opt), title('Plant P')
% hold off, grid on
% ax(2) = subplot('Position', pos_bode(2,:));
% opt.YLimMode = {'auto'}; opt.MagScale = 'linear';
% bodemag(ax(2), C_T * C_Guw, 'k', omega_bode, opt), title(''), ylabel('Coherence')
% linkaxes(ax, 'x'), clear ax
% set(findall(gcf, 'type', 'line'), 'linewidth', linewidth)
% % compare analytical to estimated controllers
% figure(expand_multiple_figure_nr(8, multp_fig_nr))
% opt.YLim = {[1e-2 1e2], [-180 180]}; opt.MagScale = 'log';
% bode(Cpi, Cd, Cpi_ana, Cd_ana, omega_bode, opt), title('Cpi, Cd')
% set(findall(gcf, 'type', 'line'), 'linewidth', linewidth)
%% new controller and filter parameters
tic
pid_axis = {'rollPID', 'pitchPID', 'yawPID'};
% PID parameters
fprintf(' used PID parameters are:\n');
fprintf([' ', pid_axis{ind_ax}, ': %d, %d, %d\n'], ...
para.(pid_axis{ind_ax})(1:3));
% inform user about parameters
para_used_fieldnames = fieldnames(para_used);
Npara_used = size(para_used_fieldnames, 1);
fprintf(' used parameters are:\n');
for i = 1:Npara_used
fprintf([' ', para_used_fieldnames{i},': %d\n'], eval(['round(', 'para_used.', para_used_fieldnames{i}, ');']));
end
% copy parameters (in case you dont change anything)
para_new = para;
% you can use the following command to generate the text
% get_switch_case_text_from_para(para)
switch quad
case 'apex5'
% type: 0: PT1, 1: BIQUAD, 2: PT2, 3: PT3
para_new.gyro_lpf = 0; % dono what this is
para_new.gyro_lowpass_hz = 0; % frequency of gyro lpf 1
para_new.gyro_soft_type = 0; % type of gyro lpf 1
para_new.gyro_lowpass_dyn_hz = [0, 0]; % dyn gyro lpf overwrites gyro_lowpass_hz
para_new.gyro_lowpass2_hz = 800; % frequency of gyro lpf 2
para_new.gyro_soft2_type = 0; % type of gyro lpf 2
para_new.gyro_notch_hz = [0, 0]; % frequency of gyro notch 1 and 2
para_new.gyro_notch_cutoff = get_fcut_from_D_and_fcenter([0.00, 0.00], para_new.gyro_notch_hz); % damping of gyro notch 1 and 2
para_new.dterm_lpf_hz = 0; % frequency of dterm lpf 1
para_new.dterm_filter_type = 0; % type of dterm lpf 1
para_new.dterm_lpf_dyn_hz = [0, 0]; % dyn dterm lpf overwrites dterm_lpf_hz
para_new.dterm_lpf2_hz = 120; % frequency of dterm lpf 2
para_new.dterm_filter2_type = 2; % type of dterm lpf 2
para_new.dterm_notch_hz = 240; % frequency of dterm notch
para_new.dterm_notch_cutoff = get_fcut_from_D_and_fcenter(0.10, para_new.dterm_notch_hz); % damping of dterm notch
para_new.yaw_lpf_hz = 200; % frequency of yaw lpf (pt1)
switch ind_ax
case 1 % roll: [55, 84, 25, 25, 0]
P_new = 56;
I_ratio_new = 80/80;
D_new = 28;
case 2 % pitch: [60, 97, 29, 29, 0]
P_new = 60;
I_ratio_new = 97/97;
D_new = 29;
case 3 % yaw: [38, 90, 3, 3, 0]
P_new = 38;
I_ratio_new = 90/90;
D_new = 3;
otherwise
end
otherwise
warning(' no valid quad selected');
end
% scale to new PID parameters
pid_scale = [get_pid_scale(ind_ax), 1];
PID_new(1) = P_new * pid_scale(1);
fI = PID(2) / (2 * pi * PID(1)); % extract fn from initial parametrization
fI_new = fI * I_ratio_new;
PID_new(2) = 2 * pi * PID_new(1) * fI_new;
PID_new(3) = D_new * pid_scale(3);
PID_new(4) = 0;
fprintf(' used fI is: %0.2f Hz\n\n', fI);
% new PID parameters
fprintf(' new PID parameters are:\n');
para_new.(pid_axis{ind_ax}) = round( PID_new ./ pid_scale);
para_new.(pid_axis{ind_ax}) = [para_new.(pid_axis{ind_ax})(1:3), ...
para_new.(pid_axis{ind_ax})(3), ...
para_new.(pid_axis{ind_ax})(4)];
fprintf([' ', pid_axis{ind_ax}, ': %d, %d, %d\n'], ...
para_new.(pid_axis{ind_ax})(1:3));
[Cpi_ana_new, Cd_ana_new, Gf_ana_new, PID_new, para_used_new] = ...
calculate_transfer_functions(para_new, ind_ax, throttle_avg, Ts_cntr);
% inform user about new parameters
para_used_fieldnames_new = fieldnames(para_used_new);
Npara_used_new = size(para_used_fieldnames_new, 1);
fprintf(' new parameters are:\n');
for i = 1:Npara_used_new
fprintf([' ', para_used_fieldnames_new{i},': %d\n'], ...
eval(['round(', 'para_used_new.', para_used_fieldnames_new{i}, ');']));
end
fprintf(' new used fI is: %0.2f Hz\n\n', fI_new);
% downsample analytical controller transferfunction and convert to frd objects
if Gf_ana_new.Ts < Ts_log % by using Gf_ana.Ts we secure that we do this only once
Gf_ana_new = downsample_frd(Gf_ana_new , Ts_log, P.Frequency);
Cpi_ana_new = downsample_frd(Cpi_ana_new, Ts_log, P.Frequency);
Cd_ana_new = downsample_frd(Cd_ana_new , Ts_log, P.Frequency);
end
CL_ana = calculate_closed_loop(Cpi_ana , tf(1,1,Ts_log), P / Gf_ana, Gf_ana , Cd_ana );
CL_ana_new = calculate_closed_loop(Cpi_ana_new, tf(1,1,Ts_log), P / Gf_ana, Gf_ana_new, Cd_ana_new);
if do_compensate_iterm
% compensate only PI part
Cpi_com = Cpi / Cpi_ana; %#ok
CL_ana_ = calculate_closed_loop(Cpi_ana * Cpi_com, tf(1,1,Ts_log), P / Gf_ana, Gf_ana , Cd_ana );
CL_ana_new_ = calculate_closed_loop(Cpi_ana_new * Cpi_com, tf(1,1,Ts_log), P / Gf_ana, Gf_ana_new, Cd_ana_new);
CL_ana.T = CL_ana_.T;
CL_ana_new.T = CL_ana_new_.T;
end
% % closed-loop
% figure(expand_multiple_figure_nr(5, multp_fig_nr))
% ax(1) = subplot(221);
% opt.YLim = {[1e-3 1e1], [-180 180]}; opt.MagScale = 'log';
% bodemag(ax(1), CL_ana.T , CL_ana_new.T , T, omega_bode, opt), title('Tracking T')
% if do_insert_legends, legend('actual', 'new', 'location', 'best'), end %#ok
% ax(2) = subplot(222);
% bodemag(ax(2), CL_ana.S , CL_ana_new.S , omega_bode, opt), title('Sensitivity S')
% ax(3) = subplot(223);
% opt.YLim = {[1e-2 1e2], [-180 180]};
% bodemag(ax(3), CL_ana.SC, CL_ana_new.SC, omega_bode, opt), title('Controller Effort SC')
% ax(4) = subplot(224);
% opt.YLim = {[1e-4 1e0], [-180 180]};
% bodemag(ax(4), CL_ana.SP, CL_ana_new.SP, omega_bode, opt), title('Compliance SP')
% linkaxes(ax, 'x'), clear ax
% set(findall(gcf, 'type', 'line'), 'linewidth', linewidth)
% step responses
f_max = para.dyn_notch_min_hz; % needs to be set according to the coherence, if inf then it has no effect
T_mean = 0.2 * [-1, 1] + (Nest * Ts_log) / 2;
step_time = (0:Nest-1).'*Ts_log;
step_resp = [calculate_step_response_from_frd(CL_ana.T , f_max), ...
calculate_step_response_from_frd(CL_ana_new.T, f_max), ...
calculate_step_response_from_frd(T , f_max)];
step_resp_mean = mean(step_resp(step_time > T_mean(1) & step_time < T_mean(2),:));
step_resp = step_resp ./ step_resp_mean;
figure(expand_multiple_figure_nr(6, multp_fig_nr))
ax(1) = subplot(211);
plot(ax(1), step_time, step_resp), grid on, ylabel('Gyro (deg/sec)')
title('Tracking T')
if do_insert_legends, legend('actual', 'new', 'location', 'best'), end %#ok
ylim([0 1.3])
step_resp = [calculate_step_response_from_frd(CL_ana.SP , f_max), ...
calculate_step_response_from_frd(CL_ana_new.SP, f_max)];
step_resp_mean = mean(step_resp(step_time > T_mean(1) & step_time < T_mean(2),:));
step_resp = step_resp - step_resp_mean;
ax(2) = subplot(212);
plot(ax(2), step_time, step_resp), grid on
title('Compliance SP'), xlabel('Time (sec)'), ylabel('Gyro (deg/sec)')
ylim([-0.2 1.1])
linkaxes(ax, 'x'), clear ax, xlim([0 0.5])
set(findall(gcf, 'type', 'line'), 'linewidth', linewidth)
% % controllers
% figure(expand_multiple_figure_nr(7, multp_fig_nr))
% opt.YLim = {[1e-1 1e2], [-180 180]};
% bode(CL_ana.C, CL_ana_new.C, omega_bode, opt)
% title('Controller C')
% if do_insert_legends, legend('actual', 'new', 'location', 'best'), end %#ok
% set(findall(gcf, 'type', 'line'), 'linewidth', linewidth)
% group and phase delay tracking
freq = CL_ana.T.Frequency;
phase = unwrap(angle(squeeze(CL_ana.T.ResponseData)));
group_delay = -gradient(phase, 2*pi*freq);
phase_delay = -phase ./ (2*pi*freq);
phase_new = unwrap(angle(squeeze(CL_ana_new.T.ResponseData)));
group_delay_new = -gradient(phase_new, 2*pi*freq);
phase_delay_new = -phase_new ./ (2*pi*freq);
figure(expand_multiple_figure_nr(8, multp_fig_nr))
subplot(121)
semilogx(freq(ind_freq), [group_delay(ind_freq), group_delay_new(ind_freq)] * 1e3), grid on
xlim([0 f_max]), ylim([0 30])
ylabel('Group Delay (ms)'), xlabel('Frequency (Hz)'), title('Group Delay T')
if do_insert_legends, legend('actual', 'new', 'location', 'best'), end %#ok
subplot(122)
semilogx(freq(ind_freq), [phase_delay(ind_freq), phase_delay_new(ind_freq)] * 1e3), grid on
xlim([0 f_max]), ylim([0 30])
xlabel('Frequency (Hz)'), ylabel('Phase Delay (ms)'), title('Phase Delay Tracking T')
if do_insert_legends, legend('actual', 'new', 'location', 'best'), end %#ok
set(findall(gcf, 'type', 'line'), 'linewidth', linewidth)
% % bode open-loop L
% figure(expand_multiple_figure_nr(9, multp_fig_nr))
% opt.YLim = {[1e-2 1e2], [-180 180]}; opt.MagScale = 'log';
% bode(CL_ana.L, CL_ana_new.L, omega_bode, opt), title('Open-Loop L')
% if do_insert_legends, legend('actual', 'new', 'location', 'best'), end %#ok
% set(findall(gcf, 'type', 'line'), 'linewidth', linewidth)
toc
% Elapsed time is 15.670831 seconds.
%%
% % amp = [para.chirp_amplitude_roll; ...
% % para.chirp_amplitude_pitch; ...
% % para.chirp_amplitude_yaw];
%
% freq_ll = [para.chirp_lead_freq_hz, para.chirp_lag_freq_hz];
%
% Gll = get_filter('leadlag1', ...
% freq_ll, ...
% Ts_log);
%
% % amp_new = amp;
%
% scaler = 1 + 1/3;
% freq_ll_new(2) = scaler*freq_ll(2); % zero
% freq_ll_new(1) = 2/3*scaler*freq_ll(1); % pole
%
% freq_ll_new = round(freq_ll_new);
%
% Gll_new = get_filter('leadlag1', ...
% freq_ll_new, ...
% Ts_log);
%
% G = tf(Gll_new / Gll);
% data_new = zeros(size(data(:,ind.motor)));
% for motor_nr = 1:length(ind.motor)
% data_new(:,motor_nr) = filter(G.num{1}, G.den{1}, ...
% data(:,ind.motor(motor_nr)));
% end
%
% figure(10)
% ax(1) = subplot(221);
% plot(ax(1), time, data(:,ind.motor)), grid on, ylabel('Motor')
% ax(2) = subplot(223);
% plot(ax(2), time, data_new), grid on, ylabel('Motor')
% linkaxes(ax, 'xy'), clear ax, xlim([0, time(end)]), ylim([0 2000])
% set(findall(gcf, 'type', 'line'), 'linewidth', linewidth)
% subplot(122)
% bode(Gll, Gll_new, G), grid on
%
% freq_ll
% freq_ll_new
Paul
Paul 2024-3-21
For this line of code (assuming it's the line causing a problem)
bode(ax(1), P / Gf_ana, 'k', omega_bode, opt), title('Plant P')
Please save P, Gf_ana, omega_bode, and opt in a .mat file and upload the .mat file using the Paperclip icon on the Insert menu.

请先登录，再进行评论。

请先登录，再回答此问题。

回答（0 个）

类别

在 帮助中心 和 File Exchange 中查找有关 Plot Customization 的更多信息

产品

版本

R2023b

标签

提问:

Stephen
Stephen
2024-3-20

评论：

Paul
Paul
2024-3-21

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by