Fminsearch to minimize norm of rms vector and thus giving me an optimized A and B matrix
1 visualizzazione (ultimi 30 giorni)
Mostra commenti meno recenti
%% Function file to minimize the A matrix and B matrix
% Need 4 inputs to predict the state variable values
clc
clear all
close all
Test_A = [-0.226952659259951, 0.0745746312124993, 0.00890963205964113, 0.0172172070742818; -8.51591268438146e-05, -0.588615273531104, 0.351669171179310, 0.00368617710122976; 0.00303570015200450, -9.95089687837753, -1.78460846839153, -0.131402573056676; 0, 0, 1, 0]; % Input a 4 by 4 matrix A
Test_B = [-0.0029; -1.09046351129058e-06; 3.88721722458518e-05; 0 ] ; % Input a 4 by 1 matrix B, only with coefficients of elevator angle
% Test_B = [0.0223, -0.0029; -0.6153, -1.09046351129058e-06; -2.3550, 3.88721722458518e-05; 0 0] ;
Test_C = eye(4);
Test_D = zeros(4,1);
Input_Data_2019_06_13 = readmatrix('actuators_node.fins.dat'); %Actuator fins data file contains the elevator angle and rudder angle input values in form of Starboards side and Bottom fins respectively
Input_Data_2019_06_13(:,1) = Input_Data_2019_06_13(:,1) - Input_Data_2019_06_13(1,1) ;
Elevator_Angle = Input_Data_2019_06_13(:,3);
% Rudder_Angle = Input_Data_2019_06_13(:,5);
% U = [Rudder_Angle, Elevator_Angle];
U = [Elevator_Angle];
U(isnan(U)) = 0; % To convert all the NaN elements in U matrix to 0
t_old = Input_Data_2019_06_13(:,1);
t_new = transpose(linspace(0,294,length(Input_Data_2019_06_13))) ; % t = linspace(0, 10, 100);
U_new = interp1(t_old,U,t_new);
U_new(isnan(U_new)) = 0;
Test_U = U_new; % Actual Field data input values
% Parsing Actual Output Data from the field
Output_surge_heave_sway_2019_06_13 = readmatrix('linkquest_dvl_node.velocity.dat');
Output_roll_pitch_yaw_Angle_2019_06_13 = readmatrix('microstrain_ahrs_node.attitude.dat');
Output_roll_pitch_yaw_Rate_2019_06_13 = readmatrix('microstrain_ahrs_node.gyroscope.dat');
Output_surge_heave_sway_2019_06_13(:,1) = Output_surge_heave_sway_2019_06_13(:,1) - Output_surge_heave_sway_2019_06_13(1,1);
Output_roll_pitch_yaw_Angle_2019_06_13(:,1) = Output_roll_pitch_yaw_Angle_2019_06_13(:,1) - Output_roll_pitch_yaw_Angle_2019_06_13(1,1);
Output_roll_pitch_yaw_Rate_2019_06_13(:,1) = Output_roll_pitch_yaw_Rate_2019_06_13(:,1) - Output_roll_pitch_yaw_Rate_2019_06_13(1,1);
% Also Interpolating the actual field output data because the data set has
% a different length and would cause difficulty while calculating error. So
% need to generalise the actual output with experimental output
Surge = Output_surge_heave_sway_2019_06_13(:,2);
Heave = Output_surge_heave_sway_2019_06_13(:,4);
Output_time1 = Output_surge_heave_sway_2019_06_13(:,1);
Output_time1_new = transpose(linspace(0,294,length(Input_Data_2019_06_13)));
Surge_field = (interp1(Output_time1,Surge,Output_time1_new));
Surge_field(isnan(Surge_field)) = 0;
Heave_field = (interp1(Output_time1,Heave,Output_time1_new));
Heave_field(isnan(Heave_field)) = 0;
Pitch_Angle = Output_roll_pitch_yaw_Angle_2019_06_13(:,3);
Output_time2 = Output_roll_pitch_yaw_Angle_2019_06_13(:,1);
Output_time2_new = transpose(linspace(0,294,length(Input_Data_2019_06_13)));
Pitch_Angle_field = (interp1(Output_time2,Pitch_Angle,Output_time2_new));
Pitch_Angle_field(isnan(Pitch_Angle_field)) = 0;
Pitch_Rate = Output_roll_pitch_yaw_Rate_2019_06_13(:,3);
Output_time3 = Output_roll_pitch_yaw_Rate_2019_06_13(:,1);
Output_time3_new = transpose(linspace(0,294,length(Input_Data_2019_06_13)));
Pitch_Rate_field = (interp1(Output_time3,Pitch_Rate,Output_time3_new));
Pitch_Rate_field(isnan(Pitch_Rate_field)) = 0;
sys = ss(Test_A,Test_B,Test_C,Test_D);
Test_t = transpose(linspace(0,294,length(Input_Data_2019_06_13))) ;
Test_SV = [2; 0; 0; 0]; % State Variable operating point values
Test_Y = lsim(ss(Test_A,Test_B,Test_C,Test_D),Test_U,Test_t,Test_SV);
rms_surge = sqrt((sum((Test_Y(:,1) - Surge_field).^ 2))/length(Test_Y(:,1)));
rms_heave = sqrt((sum((Test_Y(:,2) - Heave_field).^ 2))/length(Test_Y(:,2)));
rms_pitch_angle = sqrt((sum((Test_Y(:,3) - Pitch_Angle_field).^ 2))/length(Test_Y(:,3)));
rms_pitch_rate = sqrt((sum((Test_Y(:,4) - Pitch_Rate_field).^ 2))/length(Test_Y(:,4)));
rms = [rms_surge, rms_heave, rms_pitch_angle, rms_pitch_rate];
Output_rms = norm(rms);
X_0 = [1 0 0 0, 0 1 0 0, 0 1 0 1, 0 0 1 0, 0.01, 0.01, 0.001, 0];
% Random = RMSE(X_0,Test_C,Test_D,Test_U,Test_t,Test_SV, Surge_field, Heave_field, Pitch_Angle_field, Pitch_Rate_field);
X_Min = fminsearch(RMSE(X_0,Test_C,Test_D,Test_U,Test_t,Test_SV, Surge_field, Heave_field, Pitch_Angle_field, Pitch_Rate_field),X_0);
function Output_rms = RMSE(X_Min,Test_C,Test_D,Test_U,Test_t,Test_SV, Surge_field, Heave_field, Pitch_Angle_field, Pitch_Rate_field)
New_A(1,1:4) = X_Min(1,1:4);
New_A(2,1:4) = X_Min(1,5:8);
New_A(3,1:4) = X_Min(1,9:12);
New_A(4,1:4) = X_Min(1,13:16);
New_B(1:4,1) = X_Min(1,17:20);
Test_Y1 = lsim(ss(New_A,New_B,Test_C,Test_D),Test_U,Test_t,Test_SV);
rms_surge = sqrt((sum((Test_Y1(:,1) - Surge_field).^ 2))/length(Test_Y1(:,1)));
rms_heave = sqrt((sum((Test_Y1(:,2) - Heave_field).^ 2))/length(Test_Y1(:,2)));
rms_pitch_angle = sqrt((sum((Test_Y1(:,3) - Pitch_Angle_field).^ 2))/length(Test_Y1(:,3)));
rms_pitch_rate = sqrt((sum((Test_Y1(:,4) - Pitch_Rate_field).^ 2))/length(Test_Y1(:,4)));
rms = [rms_surge, rms_heave, rms_pitch_angle, rms_pitch_rate];
Output_rms = norm(rms);
end
2 Commenti
John D'Errico
il 18 Ott 2019
Modificato: John D'Errico
il 18 Ott 2019
What is your question? Is there a reason why you posted this, besides having a lot of spare time on your hands?
Does the code run? If so, then why are you wasting our time to read it?
If it does not run, then does it fail, with an error? If so, then why did you not tell us what the error message was? Give the COMPLETE ERROR MESSAGE, EVERYTHING IN RED.
Does it run, and give you something that you feel to be strange? If so, then tell us what it does, and then tell us WHY you think the result does not seem correct to you.
If you want help, then make it easy for someone to help you. Don't make someone spend a significant amount of time trying to guess what and why you think you have a problem. That just ensures that nobody will bother to spend the time.
Risposta accettata
Matt J
il 18 Ott 2019
fun=@(X) RMSE(X,Test_C,Test_D,Test_U,Test_t,Test_SV, Surge_field, Heave_field, Pitch_Angle_field, Pitch_Rate_field)
X_Min = fminsearch(fun,X_0);
Più risposte (0)
Vedere anche
Categorie
Scopri di più su Introduction to Installation and Licensing in Help Center e File Exchange
Prodotti
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!