Homec4science
Diffusion test_package (simple_kite) Browse

R1517/scripts/matlab/kite_sim.m
bbf13bc40b94simple_kite

Last Change
Raw File

kite_sim.m
View Options

function [NUM, FLOG, SYM] = kite_sim(aircraft, params)
%casadi based kite dynamics simulation
import casadi.*
%-------------------------
%Enviromental constants
%-------------------------
g = 9.80665; % gravitational acceleration [m/s2] [WGS84]
ro = 1.2985; % standart atmospheric density [kg/m3] [Standart Atmosphere 1976]
%---------------------------
%Glider geometric parameters
%---------------------------
b = aircraft.geometry.b;
c = aircraft.geometry.c;
AR = aircraft.geometry.AR;
S = aircraft.geometry.S;
lam = aircraft.geometry.lam;
St = aircraft.geometry.St;
lt = aircraft.geometry.lt;
Sf = aircraft.geometry.Sf;
lf = aircraft.geometry.lf;
Xac = aircraft.geometry.Xac;
Xcg = 0.031/c; % Center of Gravity (CoG) wrt leading edge [1/c]
Vf = (Sf * lf) / (S * b); % fin volume coefficient []
Vh = (lt * St) / (S * c); % horizontal tail volume coefficient []
%---------------------------
%Mass and inertia parameters
%---------------------------
Mass = aircraft.inertia.mass;
Ixx = aircraft.inertia.Ixx;
Iyy = aircraft.inertia.Iyy;
Izz = aircraft.inertia.Izz;
Ixz = aircraft.inertia.Ixz;
%-------------------------------
%Static aerodynamic coefficients
%-------------------------------
% All characteristics assumed linear
CL0 = aircraft.aerodynamic.CL0;
CL0_t = aircraft.aerodynamic.CL0_tail;
CLa_tot = aircraft.aerodynamic.CLa_total;
CLa_w = aircraft.aerodynamic.CLa_wing;
CLa_t = aircraft.aerodynamic.CLa_tail;
e_o = aircraft.aerodynamic.e_oswald;
dw = CLa_tot / (pi * e_o * AR); % downwash acting at the tail []
CD0_tot = aircraft.aerodynamic.CD0_total;
CD0_w = aircraft.aerodynamic.CD0_wing;
CD0_t = aircraft.aerodynamic.CD0_tail;
CYb = aircraft.aerodynamic.CYb;
CYb_vt = aircraft.aerodynamic.CYb_vtail;
Cm0 = aircraft.aerodynamic.Cm0;
Cma = aircraft.aerodynamic.Cma;
Cn0 = aircraft.aerodynamic.Cn0;
Cnb = aircraft.aerodynamic.Cnb;
Cl0 = aircraft.aerodynamic.Cl0;
Clb = aircraft.aerodynamic.Clb;
CLq = aircraft.aerodynamic.CLq;
Cmq = aircraft.aerodynamic.Cmq;
CYr = aircraft.aerodynamic.CYr;
Cnr = aircraft.aerodynamic.Cnr;
Clr = aircraft.aerodynamic.Clr;
CYp = aircraft.aerodynamic.CYp;
Clp = aircraft.aerodynamic.Clp;
Cnp = aircraft.aerodynamic.Cnp;
%------------------------------
%Aerodynamic effects of control
%------------------------------
CLde = aircraft.aerodynamic.CLde;
CYdr = aircraft.aerodynamic.CYdr;
Cmde = aircraft.aerodynamic.Cmde;
Cndr = aircraft.aerodynamic.Cndr;
Cldr = aircraft.aerodynamic.Cldr;
CDde = aircraft.aerodynamic.CDde; % (assume negligible)
CL_daoa = -2 * CLa_t * Vh * dw; % aoa-rate effect on lift (from Etkin) [] Stengel gives positive estimation !!!
Cm_daoa = -2 * CLa_t * Vh * (lt/c) * dw; %aoa-rate effect on pitch moment [] Stengel gives positive estimation !!!
%--------------------------
%State variables definition
%--------------------------
r = SX.sym('r', 3); % position of the CoG in the Inertial Reference Frame (IRF) [m]
q = SX.sym('q', 4); % body attitude relative to IRF [unit quaternion]
v = SX.sym('v', 3); % linear velocity of the CoG in the Body Reference Frame (BRF) [m/s]
w = SX.sym('w', 3); % glider angular velocity in BRF [rad/s]
%----------------------------
%Control variables definition
%----------------------------
%TODO: more detatailed propeller model will be added later
T = SX.sym('T'); % propeller propulsion : applies along X-axis in BRF [N]
dE = SX.sym('dE'); % elevator deflection [positive down] [rad]
dR = SX.sym('dR'); % rudder deflection [rad]
dA = SX.sym('dA'); % aileron deflection [reserved, but not used] [rad]
dF = SX.sym('dF'); % flaps deflection [reserved, but not used]
%Read algorithm and simulation parameters
%DEFAULTS
SIMULATION = 0;
USE_CVODES = 1;
VR_SIM = 0;
PLOT_RESULTS = 0;
X0 = [];
U0 = [];
T_START = 0.0;
T_FINISH = 1.0;
DT = 0.01;
if(isfield(params, 'simulation'))
SIMULATION = params.simulation;
end
if(isfield(params, 'int_type'))
if(isequal(params.int_type,'cvodes'))
USE_CVODES = 1;
else if (isequal(params.int_type, 'rk4'))
USE_CVODES = 0;
else
fprintf(2, 'ERROR: Unknown integrator type: [%s] \n', params.int_type);
error('Use "cvodes" or "rk4" instead');
end
end
end
%simulation time span
if(isfield(params, 't_span'))
if(numel(params.t_span) ~= 3)
error('ERROR: time span format should be: [start, finish, dt]');
else
T_START = params.t_span(1);
T_FINISH = params.t_span(2);
DT = params.t_span(3);
if(T_START > T_FINISH)
error('ERROR: T_START > T_FINISH');
end
end
end
if(SIMULATION)
%VR visualisation
if(isfield(params, 'vr'))
VR_SIM = params.vr;
end
%initial condition and input
if(isfield(params,'x0') && isfield(params,'u0'))
X0 = params.x0;
U0 = params.u0;
else
error('ERROR: initial conditions "x0" and control "u0" should be specified for simulation');
end
if(isfield(params,'plot'))
PLOT_RESULTS = params.plot;
end
end
%TODO: for now assume there is no wind, this functionality will be added later
V = L2(v);
V2 = v' * v;
ss = asin(v(2) / V); %side slip angle [rad] (v(3)/v(1)) // small angle assumption
aoa = atan2(v(3) , v(1)); %angle of attack definition [rad] (v(2)/L2(v))
dyn_press = 0.5 * ro * V2; %dynamic pressure
CD_w = CD0_w + (CL0 + CLa_w * aoa )^2 / (pi * e_o * AR); %wing drag coefficient
CD = CD0_tot + (CL0 + CLa_tot * aoa)^2 / (pi * e_o * AR); %total drag coefficient
%-------------------------
%Dynamic Equations: Forces
%-------------------------
LIFT = (CL0 + CLa_tot * aoa) * dyn_press * S + ...
(0.25 * CLq * c * S * ro) * V * w(2);
DRAG = CD * dyn_press * S;
SF = (CYb * ss + CYdr * dR) * dyn_press * S + ...
0.25 * (CYr * w(3) + CYp * w(1)) * (b * ro * S) * V;
%Compute transformation betweeen WRF and BRF: qw_b
%qw_b = q(aoa) * q(-ss);
q_aoa = [cos(aoa/2); 0; sin(aoa/2); 0];
q_ss = [cos(-ss/2); 0; 0; sin(-ss/2)];
qw_b = quat_mul(q_aoa, q_ss);
qw_b_inv = quat_inv(qw_b);
%The following is a complete disaster
%Aerodynamic forces in BRF: Faer0_b = qw_b * [0; -DRAG; SF; -LIFT] * qw_b_inv
qF_tmp = quat_mul(qw_b_inv, [0; -DRAG; 0; -LIFT]);
qF_q = quat_mul(qF_tmp, qw_b);
Faero_b = qF_q(2:4);
Zde = (-CLde) * dE * dyn_press * S;
FdE_tmp = quat_mul(quat_inv(q_aoa), [0; 0; 0; Zde]);
FdE = quat_mul(FdE_tmp, q_aoa);
FdE = FdE(2:4);
Faero_b = Faero_b + FdE + [0; SF; 0];
%Gravity force in BRF
qG = quat_mul(quat_inv(q),[0;0;0;g]);
qG_q = quat_mul(qG, q);
G_b = qG_q(2:4);
%Propulsion force in BRF
T_b = [T;0;0];
%Tether force
%value: using smooth ramp approximation
Lt = 2.31; %tether length
d_ = L2(r);
%spring term
%Rv = ((d_ - Lt)) * Heaviside(d_ - Lt, 1);
Ks = 500 * Mass; %300 Ks: 800;
Kd = 10 * Mass; %30 Kd: 500
Rv = ((d_ - Lt));
Rs = -Rv * (r / d_);
%damping term
qvi = quat_mul(q, [0; v]);
qvi_q = quat_mul(qvi, quat_inv(q));
vi = qvi_q(2:4);
Rd = (-r / d_) * (r' * vi) / d_;
R = ( Ks * Rs + Kd * Rd) * Heaviside(d_ - Lt, 1);
%BRF
qR = quat_mul(quat_inv(q),[0;R]);
qR_q = quat_mul(qR, q);
R_b = qR_q(2:4);
%Total external forces devided by glider's mass (linear acceleration)
v_dot = (Faero_b + T_b + R_b)/Mass + G_b - cross(w,v);
%-------------------------
%Dynamic Equation: Moments
%-------------------------
%Rolling Aerodynamic Moment
L = (Cl0 + Clb * ss + Cldr * dR) * dyn_press * S * b + ...
(Clr * w(3) + Clp * w(1)) * (0.25 * ro * b^2 * S) * V;
%Pitching Aerodynamic Moment
M = (Cm0 + Cma * aoa + Cmde * dE) * dyn_press * S * c + ...
Cmq * (0.25 * S * c^2 * ro) * w(2) * V;
%Yawing Aerodynamic Moment
N = (Cn0 + Cnb * ss + Cndr * dR) * dyn_press * S * b + ...
(Cnp * w(1) + Cnr * w(3)) * (0.25 * S * b^2 * ro) * V;
%Aircraft Inertia Matrix
J = [Ixx 0 Ixz; 0 Iyy 00; Ixz 0 Izz];
%Angular motion equationin BRF
Maero = [L; M; N];
%Moments rotation SRF -> BRF
T_tmp = quat_mul(quat_inv(q_aoa), [0; Maero]);
Trot = quat_mul(T_tmp, q_aoa);
Maero = Trot(2:4);
w_dot = inv(J) * (Maero - cross(w, J * w));
%-----------------------------
%Kinematic Equations: Position
%-----------------------------
% Aircraft position in the IRF
qv = quat_mul(q, [0; v]);
qv_q = quat_mul(qv, quat_inv(q));
r_dot = qv_q(2:4);
%-----------------------------
%Kinematic Equations: Attitude
%-----------------------------
%Aircraft attitude wrt to IRF
q_dot = 0.5 * quat_mul(q, [0;w]);
%Now we completely define dynamics of the aircraft
state = [v; w; r; q];
control = [T; dE; dR];
dynamics = [v_dot; w_dot; r_dot; q_dot];
dyn_func = Function('dynamics', {state, control}, {dynamics});
%compute dynamics state Jacobian
d_jacobian = dynamics.jacobian(state);
dyn_jac = Function('dyn_jacobian',{state, control}, {d_jacobian});
%define RK4 integrator scheme
X = SX.sym('X',13);
U = SX.sym('U',3);
dT = SX.sym('dT');
%get symbolic expression for an integrator
integrator = RK4_sym(X, U, dyn_func, dT);
RK4_INT = Function('RK4', {X,U,dT},{integrator});
%get Jacobian of the RK4 Integrator
integrator_jacobian = integrator.jacobian(X);
rk4_jacobian = Function('RK4_JACOBIAN', {X, U, dT}, {integrator_jacobian});
h = DT;
x0 = X0;
u0 = U0;
%CVODES integrator
ode = struct('x',state, 'p',control, 'ode',dynamics);
opts = struct('tf', h);
CVODES_INT = casadi.integrator('CVODES_INT','cvodes', ode, opts);
aoa_func = Function('AoA',{v},{aoa});
ss_func = Function('SS',{v},{ss});
%return integrator function
if(USE_CVODES)
NUM.INT = CVODES_INT;
else
NUM.INT = RK4_INT;
end
%Symbolic expression for the model
SYM.INT = integrator;
SYM.DYNAMICS = dynamics;
SYM.DYN_JAC = d_jacobian;
SYM.state = state;
SYM.control = control;
SYM.X = X;
SYM.U = U;
SYM.dT = dT;
NUM.DYN_FUNC = dyn_func;
NUM.DYN_JACOBIAN = dyn_jac;
NUM.RK4_JACOBIAN = rk4_jacobian;
if(VR_SIM)
%create simulation scene
[scene] = CreateScene('BackView',1);
end
%-----------------------------
%Open-Loop Flight Simulation
%-----------------------------
%simulation & visualisation
FLOG = [];
if (SIMULATION)
t = T_START;
log = [];
flight_angles = [];
res = [];
while t <= T_FINISH
%simulation loop
%logging data
log = [log; x0', t];
aoa_t = aoa_func({x0(1:3)});
full(aoa_t{1});
ss_t = ss_func({x0(1:3)});
full(ss_t{1});
flight_angles = [flight_angles; full(aoa_t{1}) full(ss_t{1})];
if (USE_CVODES)
out = dyn_func({x0,u0});
dyn =full(out{1});
out = CVODES_INT(struct('x0',x0, 'p',u0));
res = full(out.xf);
else
out = RK4_INT({x0, u0, h});
res = full(out{1});
end
t = t + h;
x0 = res;
if(VR_SIM)
UpdateScene(scene, res(7:9), res(10:13),t);
end
end
%return flight telemetry
FLOG = log;
if(PLOT_RESULTS)
%trajectory
figure
plot3(log(:,7), log(:,8), -log(:,9));
axis equal
grid on
%velocities in BRF
figure('Name','Linear Velocities in BRF','NumberTitle','off')
plot(log(:,end), log(:,1:3))
grid on
%angle of attack
figure('Name','Angle of Attack & Sideslip Angle','NumberTitle','off')
%plot(rad2deg(flight_angles));
plot(log(:,end), flight_angles);
grid on
%attitude
figure('Name','Attitude (EulerAngles)','NumberTitle','off')
plot(log(:,end), quat2eul(log(:, 10:13),'ZYX'))
grid on
%angular rates
figure('Name','Angular rates in BRF','NumberTitle','off')
plot(log(:,end), log(:,4:6))
grid on
end
end
end
%Casadi helper functions
%L2-norm of a vector
function [L2] = L2(x)
if(size(x,1) >= size(x,2))
L2 = sqrt(x' * x);
else
L2 = sqrt(x * x');
end
end
%quaternion multiplication
function [q] = quat_mul(q1,q2)
s1 = q1(1);
v1 = q1(2:4);
s2 = q2(1);
v2 = q2(2:4);
s = s1*s2 - dot(v1,v2);
v = cross(v1,v2) + s1 * v2 + s2 * v1;
q = [s; v(1); v(2); v(3)];
end
%inverse quaternion
function [q_inv] = quat_inv(q)
q_inv = [q(1); -q(2); -q(3); -q(4)];
end
%Runge-Kutta 4th-order symbolic integration method
function [x_h] = RK4_sym(x, u, f, h)
% this integrator is problem specific
k1 = f({x,u});
k2 = f({x + 0.5 * h * k1{1},u});
k3 = f({x + 0.5 * h * k2{1},u});
k4 = f({x + h * k3{1},u});
x_h = x + (h/6) * (k1{1} + 2*k2{1} + 2*k3{1} + k4{1});
end
%Heaviside smooth approximation
function[H] = Heaviside(x,k)
H = k / (1 + exp(-4*x));
end
function [scene] = CreateScene(viewpoint, is_recording)
%create 3D scene
scene = struct;
scene.world = vrworld('world.wrl','new');
open(scene.world);
fig = vrfigure(scene.world);
% go to a viewpoint suitable for user navigation
set(fig, 'Viewpoint', viewpoint);
% get the manipulated airplane node
scene.position = vrnode(scene.world, 'GliderTranslational');
scene.attitude = vrnode(scene.world, 'GliderRotational');
if(is_recording)
set(scene.world, 'RecordMode', 'manual');
set(fig, 'Record2DFileName', 'flight_sim.avi');
set(fig, 'Record2D', 'on');
set(fig, 'Record2DCompressQuality', 100);
end
end
function [] = UpdateScene(scene, position, attitude, time)
%frame transformations with VR simulator
q_sim = eul2quat([ 0 0 pi/2]);
Xi = position;
Qi = attitude;
%apply simulation and draw
x_sim = quatmul(quatmul(q_sim, [0 Xi']),quatinv(q_sim));
q_disp = quatmul(quatmul(q_sim, Qi'),quatinv(q_sim));
scene.position.translation = x_sim(2:4);
scene.attitude.rotation = quat2axang(q_disp);
set(scene.world, 'Time', time);
vrdrawnow;
end
c4science · Help