I have been working with a commercial flight computer and have adjusted the gains of my controller. However, I want to perform a more in-depth analysis, which requires finding an appropriate model to fine-tune my gains. I have been experimenting with the following code:
clear;
clc;
% Load the data
load("HolomIdent160724.mat");
% Extract relevant data and convert to double
roll = double(AttitudeEuler_Roll.Var1);
t_roll = seconds(AttitudeEuler_Roll.Time);
pitch = double(AttitudeEuler_Pitch.Var1);
t_pitch = seconds(AttitudeEuler_Pitch.Time);
yaw = double(AttitudeEuler_Yaw.Var1);
t_yaw = seconds(AttitudeEuler_Yaw.Time);
v_roll = double(Gyro_GyroX.Var1);
t_v_roll = seconds(Gyro_GyroX.Time);
v_pitch = double(Gyro_GyroY.Var1);
t_v_pitch = seconds(Gyro_GyroY.Time);
v_yaw = double(Gyro_GyroZ.Var1);
t_v_yaw = seconds(Gyro_GyroZ.Time);
u_roll = double(torque_xyz_var1.Var1);
t_u_roll = seconds(torque_xyz_var1.Time);
u_pitch = double(torque_xyz_var2.Var1);
t_u_pitch = seconds(torque_xyz_var2.Time);
u_yaw = double(torque_xyz_var3.Var1);
t_u_yaw = seconds(torque_xyz_var3.Time);
% Determine the minimum time interval and increase it to reduce memory usage
min_interval = min([min(diff(t_roll)), min(diff(t_pitch)), min(diff(t_yaw)), ...
min(diff(t_v_roll)), min(diff(t_v_pitch)), min(diff(t_v_yaw)), ...
min(diff(t_u_roll)), min(diff(t_u_pitch)), min(diff(t_u_yaw))]) * 10; % Increase interval to reduce resolution
% Define a common time base with the determined minimum interval
t_common = min([t_roll(1), t_pitch(1), t_yaw(1), t_v_roll(1), t_v_pitch(1), t_v_yaw(1), t_u_roll(1), t_u_pitch(1), t_u_yaw(1)]): ...
min_interval: ...
max([t_roll(end), t_pitch(end), t_yaw(end), t_v_roll(end), t_v_pitch(end), t_v_yaw(end), t_u_roll(end), t_u_pitch(end), t_u_yaw(end)]);
% Resample the signals to the common time base
roll_resampled = interp1(t_roll, roll, t_common, 'linear', 'extrap');
pitch_resampled = interp1(t_pitch, pitch, t_common, 'linear', 'extrap');
yaw_resampled = interp1(t_yaw, yaw, t_common, 'linear', 'extrap');
v_roll_resampled = interp1(t_v_roll, v_roll, t_common, 'linear', 'extrap');
v_pitch_resampled = interp1(t_v_pitch, v_pitch, t_common, 'linear', 'extrap');
v_yaw_resampled = interp1(t_v_yaw, v_yaw, t_common, 'linear', 'extrap');
u_roll_resampled = interp1(t_u_roll, u_roll, t_common, 'linear', 'extrap');
u_pitch_resampled = interp1(t_u_pitch, u_pitch, t_common, 'linear', 'extrap');
u_yaw_resampled = interp1(t_u_yaw, u_yaw, t_common, 'linear', 'extrap');
% Trim data if necessary to reduce memory usage
max_length = 50000; % Define a maximum length for the data
if length(t_common) > max_length
t_common = t_common(1:max_length);
roll_resampled = roll_resampled(1:max_length);
pitch_resampled = pitch_resampled(1:max_length);
yaw_resampled = yaw_resampled(1:max_length);
v_roll_resampled = v_roll_resampled(1:max_length);
v_pitch_resampled = v_pitch_resampled(1:max_length);
v_yaw_resampled = v_yaw_resampled(1:max_length);
u_roll_resampled = u_roll_resampled(1:max_length);
u_pitch_resampled = u_pitch_resampled(1:max_length);
u_yaw_resampled = u_yaw_resampled(1:max_length);
end
% Combine the data into matrices
y = [roll_resampled', v_roll_resampled', pitch_resampled', v_pitch_resampled', yaw_resampled', v_yaw_resampled'];
u = [u_roll_resampled', u_pitch_resampled', u_yaw_resampled'];
time_intervals1 = diff(t_common);
average_interval_ms1 = mean(time_intervals1) * 1000; % Convert to milliseconds
average_interval_s1 = mean(time_intervals1);
sampling_frequency1 = 1 / average_interval_s1;
% Apply lowpass filter to v_roll
[v_roll_resampled, d] = lowpass(v_roll_resampled, 250, sampling_frequency1);
y_roll = [roll_resampled', v_roll_resampled'];
% Create iddata object
dron1 = iddata(y, u, average_interval_s1);
roll_1 = iddata(y_roll, u_roll_resampled', average_interval_s1);
% Plot to verify the trimming
figure(1);
subplot(3, 1, 1);
plot(t_common, roll_resampled, t_common, pitch_resampled, t_common, yaw_resampled);
title('Attitude Euler Angles');
legend('Roll', 'Pitch', 'Yaw');
subplot(3, 1, 2);
plot(t_common, v_roll_resampled, t_common, v_pitch_resampled, t_common, v_yaw_resampled);
title('Gyroscope Rates');
legend('Roll Rate', 'Pitch Rate', 'Yaw Rate');
subplot(3, 1, 3);
plot(t_common, u_roll_resampled, t_common, u_pitch_resampled, t_common, u_yaw_resampled);
title('Control Inputs');
legend('Roll Control', 'Pitch Control', 'Yaw Control');
% Define the non-linear grey box model
FileName = 'dronmodel_m'; % File describing the model structure.
Order = [6 3 6]; % Model orders [ny nu nx].
Parameters = [5;5;5]; % Initial parameters [Inertia; drag].
InitialStates = [0; 0; 0; 0; 0; 0]; % Initial initial states [roll_angle; roll_velocity].
Ts = average_interval_s1; % Time-continuous system.
nlgr = idnlgrey(FileName, Order, Parameters, InitialStates, Ts, 'Name', 'RollTF');
figure(2)
compare(dron1, nlgr)
And my model is :
function [dx, y] = dronmodel_m(~, x, u,Ixx,Iyy,Izz,varargin)
%Extract states and parameters
phiR= x(1);%roll_angle
dphiR = x(2);%roll_velocity
thetaR = x(3);%pitch_angle
dthetaR = x(4);%pitch_velocity
psiR = x(5);%yaw_angle
dpsiR = x(6);%yaw_velocity
Tx=u(1);
Ty=u(2);
Tz=u(3);
% inertia = p(1);
% drag = p(2);
J=[Ixx,0,-Ixx*sin(thetaR);0,Iyy*((cos(phiR))^2)+Izz*(sin(phiR))^2,Iyy*cos(thetaR)*cos(phiR)*sin(phiR)-Izz*cos(thetaR)*cos(phiR)*sin(phiR);-Ixx*sin(thetaR),Iyy*cos(thetaR)*cos(phiR)*sin(phiR)-Izz*cos(thetaR)*cos(phiR)*sin(phiR),(Izz*(cos(thetaR))^2)*((cos(phiR))^2)+Ixx*((sin(thetaR))^2)+Iyy*((cos(thetaR))^2)*(sin(phiR))^2];
Tau=[Tx;Ty;Tz];
eta=[phiR;thetaR;psiR];
deta=[dphiR;dthetaR;dpsiR];
C33= Ixx*dthetaR*sin(thetaR)*cos(thetaR)+Iyy*(-dthetaR*sin(thetaR)*cos(thetaR)*((sin(phiR))^2)+dphiR*((cos(thetaR))^2)*sin(phiR)*cos(phiR))-Izz*(dthetaR*sin(thetaR)*cos(thetaR)*((cos(phiR))^2)+dphiR*((cos(thetaR))^2)*sin(phiR)*cos(phiR));
C32=Ixx*dpsiR*sin(thetaR)*cos(thetaR)-Iyy*(dthetaR*sin(thetaR)*sin(phiR)*cos(phiR)+dphiR*cos(thetaR)*((sin(phiR))^2)-dphiR*cos(thetaR)*((cos(phiR))^2)+dpsiR*sin(thetaR)*cos(thetaR)*((sin(phiR))^2))+Izz*(dphiR*cos(thetaR)*((sin(phiR))^2)-dphiR*cos(thetaR)*((cos(phiR))^2)-dpsiR*sin(thetaR)*cos(thetaR)*((cos(phiR))^2)+dthetaR*sin(thetaR)*sin(phiR)*cos(phiR));
C31=-Ixx*dthetaR*cos(thetaR)+Iyy*dpsiR*((cos(thetaR))^2)*sin(phiR)*cos(phiR)-Izz*dpsiR*((cos(thetaR))^2)*sin(phiR)*cos(phiR);
C23=-Ixx*dpsiR*sin(thetaR)*cos(thetaR)+Iyy*dpsiR*sin(thetaR)*cos(thetaR)*((sin(phiR))^2)+Izz*dpsiR*sin(thetaR)*cos(thetaR)*((cos(phiR))^2);
C22=-Iyy*dphiR*sin(phiR)*cos(phiR)+Izz*dphiR*sin(phiR)*cos(phiR);
C21=Ixx*dpsiR*cos(thetaR)+Iyy*(-dthetaR*sin(phiR)*cos(phiR)+dpsiR*cos(thetaR)*((cos(phiR))^2)-dpsiR*cos(thetaR)*((sin(phiR))^2))+Izz*(dpsiR*cos(thetaR)*((sin(phiR))^2)-dpsiR*cos(thetaR)*((cos(phiR))^2)+dthetaR*sin(phiR)*cos(phiR));
C13=-Iyy*dpsiR*((cos(thetaR))^2)*sin(phiR)*cos(phiR)+Izz*dpsiR*((cos(thetaR))^2)*sin(phiR)*cos(phiR);
C12=-Ixx*dpsiR*cos(thetaR)+Iyy*(dthetaR*sin(phiR)*cos(phiR)+dpsiR*cos(thetaR)*((sin(phiR))^2)-dpsiR*cos(thetaR)*((cos(phiR))^2))-Izz*(dpsiR*cos(thetaR)*((sin(phiR))^2)-dpsiR*cos(thetaR)*((cos(phiR))^2)+dthetaR*sin(phiR)*cos(phiR));
C11=0;
C=[C11,C12,C13;C21,C22,C23;C31,C32,C33];
%State equations (example dynamics)
dangles = deta;
ddangles = J\(Tau-C*dangles);
%State derivatives
dx = [dangles; ddangles];
%Outputs (roll angle and velocity)
y = [eta; deta];
end
But I can not find an adequate model, how can I improve this code?
