Commit a093a983 authored by Gidon Lucian Bauer's avatar Gidon Lucian Bauer 🏳
Browse files

So ziemlich alles was ich in MATLAB geschrieben habe

parent 60021c5d
classdef aux_quadPlot_simulink < matlab.System & handle & matlab.system.mixin.Propagates
% Visualization class for quadrotor simulation
%
% Author1 : Pennsylvania State University
% Author2 : Eugen Nuss (e.nuss@irt.rwth-aachen.de)
%
% Last change :
%
% Description : Visualization class for quadrotor simulation
%
% Attributes :
%
% Methods :
%
%-------------------------------------------------------------------------%
%---------------------------------------------------------------------%
% Set attributes %
%---------------------------------------------------------------------%
properties (Access = public)
qn = 1; % Quad number
color = [0 0.447 0.741]; % Color of quad
wingspan = 0.1; % Wingspan
cstep = 0.1; % Image capture time interval/ Update step width [s]
max_time = 50;
height = 0.04; % Height of quad
text_dist = 0.03; % Distance of quad number to quad
drawspeed = 0.02;
end
properties(DiscreteState)
state_hist % Position history
state_des_hist % Desired position history
time_hist % Time history
k % Counter
k_int % Counter if xk+1 = xk+T for T = 0 or T
end
% Pre-computed constants
properties(Access = private)
h_3d % Axis-handle of 3D plot
h_xz % Axis-handle of x-z-plane
h_yz % Axis-handle of y-z-plane
h_xy % Axis-handle of x-y-plane
h_xzBody % Axis-handle of body fixed x-z-plane
h_yzBody % Axis-handle of body fixed y-z-plane
motor; % Motor position
end
%---------------------------------------------------------------------%
%---------------------------------------------------------------------%
% Define methods %
%---------------------------------------------------------------------%
methods(Access = protected)
function [sz,dt,cp] = getDiscreteStateSpecificationImpl(obj,name)
switch name
case {'state_hist', 'state_des_hist', 'time_hist'}
nCol = obj.max_time/obj.cstep;
sz = [6 nCol];
dt = 'double';
cp = false;
case {'k', 'k_int'}
sz = [1 1];
dt = 'double';
cp = false;
otherwise
error(['Error: Incorrect State Name: ', name]);
end
end
function dt1 = getOutputDataTypeImpl(~)
dt1 = 'double';
end
function cp1 = isOutputComplexImpl(~)
cp1 = false;
end
function fz1 = isOutputFixedSizeImpl(~)
fz1 = true;
end
function num = getNumOutputsImpl(~)
num = 1;
end
function sze = getOutputSizeImpl(~)
sze = [1 1];
end
function setupImpl(obj, ~, state, stateDesired, ~)
% Perform one-time calculations, such as computing constants
close all;
disp('Initializing figures...');
figure;
sp3d = subplot(3,3,[1,2,4,5]);
obj.h_3d = hggroup;
axis equal
grid on
view(3);
xlabel('x [m]'); ylabel('y [m]'); zlabel('z [m]')
spxz = subplot(333);
obj.h_xz = hggroup;
axis equal
grid on
box on
view(0,0);
xlabel('x [m]'); ylabel('y [m]'); zlabel('z [m]')
spyz = subplot(336);
obj.h_yz = hggroup;
axis equal
grid on
box on
view(90,0);
xlabel('x [m]'); ylabel('y [m]'); zlabel('z [m]')
spxy = subplot(339);
obj.h_xy = hggroup;
axis equal
grid on
box on
view(0,90);
xlabel('x [m]'); ylabel('y [m]'); zlabel('z [m]')
spxzBody = subplot(337);
obj.h_xzBody = hggroup;
axis equal
grid on
box on
view(0,0);
xlabel('x [m]'); ylabel('y [m]'); zlabel('z [m]')
spyzBody = subplot(338);
obj.h_yzBody = hggroup;
axis equal
grid on
box on
view(90,0);
xlabel('x [m]'); ylabel('y [m]'); zlabel('z [m]')
UpdateQuadState(obj, state)
% Initialize 3D plot handle
groupHandles = {obj.h_xzBody, obj.h_yzBody, obj.h_3d, ...
obj.h_xz, obj.h_yz, obj.h_xy};
subPlots = {spxzBody, spyzBody, sp3d, spxz, spyz, spxy};
for ii = 1:length(groupHandles)
hold(subPlots{ii}, 'on')
if ii>2
plot3(state(1), state(2), state(3), 'r.', 'parent', groupHandles{ii});
plot3(stateDesired(1), stateDesired(2), stateDesired(3), 'b.', 'parent', groupHandles{ii});
end
plot3(obj.motor(1,[1 3]), ...
obj.motor(2,[1 3]), ...
obj.motor(3,[1 3]), ...
'-ko', 'MarkerFaceColor', obj.color, 'MarkerSize', 5, 'parent', groupHandles{ii});
plot3(...
obj.motor(1,[2 4]), ...
obj.motor(2,[2 4]), ...
obj.motor(3,[2 4]), ...
'-ko', 'MarkerFaceColor', obj.color, 'MarkerSize', 5, 'parent', groupHandles{ii});
plot3(...
obj.motor(1,[5 6]), ...
obj.motor(2,[5 6]), ...
obj.motor(3,[5 6]), ...
'Color', obj.color, 'LineWidth', 2, 'parent', groupHandles{ii});
text(...
obj.motor(1,5) + obj.text_dist, ...
obj.motor(2,5) + obj.text_dist, ...
obj.motor(3,5) + obj.text_dist, num2str(obj.qn), 'parent', groupHandles{ii});
hold(subPlots{ii}, 'off')
end
disp('Done');
end
function UpdateQuadState(obj, state)
% Update motor position
% quadpos function
% QUAD_POS Calculates coordinates of quadrotor's position in world frame
% pos 3x1 position vector [x; y; z];
% rot 3x3 body-to-world rotation matrix
% L 1x1 length of the quad
% wRb = RPYtoRot_ZXY(euler(1), euler(2), euler(3))';
% Update quad state
[~,rot] = aux_rotEuler( zeros(3,1), state(7:9), 'to-inertial-frame');
% quadpos function
wHb = [rot state(1:3); 0 0 0 1]; % homogeneous transformation from body to world
quadBodyFrame = [obj.wingspan 0 0 1; 0 obj.wingspan 0 1; ...
-obj.wingspan 0 0 1; 0 -obj.wingspan 0 1; ...
0 0 0 1; 0 0 obj.height 1]';
quadWorldFrame = wHb * quadBodyFrame;
obj.motor = quadWorldFrame(1:3, :);
end
function UpdateQuadHist(obj, time, state, des_state)
% Update quad history
obj.k = obj.k + 1;
obj.time_hist(obj.k) = time;
obj.state_hist(:,obj.k) = state(1:6);
obj.state_des_hist(:,obj.k) = des_state(1:6);
end
% function TruncateHist(obj)
% % Truncate history
% obj.time_hist = obj.time_hist(1:obj.k);
% obj.state_hist = obj.state_hist(:, 1:obj.k);
% obj.state_des_hist = obj.state_des_hist(:, 1:obj.k);
% end
function y = stepImpl(obj, b3dPlot, state, stateDesired, time)
% Implement algorithm. Calculate y as a function of input u and
% discrete states.
iter = mod(time, obj.cstep);
if b3dPlot && iter==0 && obj.k_int==0
obj.k_int = 1;
obj.UpdateQuadState(state);
obj.UpdateQuadHist(time, state, stateDesired);
groupHandles = {obj.h_xzBody, obj.h_yzBody, obj.h_3d,...
obj.h_xz, obj.h_yz, obj.h_xy};
for ii = 1:length(groupHandles)
tmpLine = get(groupHandles{ii}, 'Children');
if ii<=2
set(tmpLine(4), ...
'XData', obj.motor(1,[2 4]), ...
'YData', obj.motor(2,[2 4]), ...
'ZData', obj.motor(3,[2 4]));
set(tmpLine(2), ...
'XData', obj.motor(1,[5 6]), ...
'YData', obj.motor(2,[5 6]), ...
'ZData', obj.motor(3,[5 6]))
set(tmpLine(3), ...
'XData', obj.motor(1,[1 3]), ...
'YData', obj.motor(2,[1 3]), ...
'ZData', obj.motor(3,[1 3]));
set(tmpLine(1), 'Position', ...
[obj.motor(1,5) + obj.text_dist, ...
obj.motor(2,5) + obj.text_dist, ...
obj.motor(3,5) + obj.text_dist]);
else
set(tmpLine(5), ...
'XData', obj.state_des_hist(1,1:obj.k), ...
'YData', obj.state_des_hist(2,1:obj.k), ...
'ZData', obj.state_des_hist(3,1:obj.k));
set(tmpLine(6), ...
'XData', obj.state_hist(1,1:obj.k), ...
'YData', obj.state_hist(2,1:obj.k), ...
'ZData', obj.state_hist(3,1:obj.k));
set(tmpLine(3), ...
'XData', obj.motor(1,[2 4]), ...
'YData', obj.motor(2,[2 4]), ...
'ZData', obj.motor(3,[2 4]));
set(tmpLine(2), ...
'XData', obj.motor(1,[5 6]), ...
'YData', obj.motor(2,[5 6]), ...
'ZData', obj.motor(3,[5 6]))
set(tmpLine(4), ...
'XData', obj.motor(1,[1 3]), ...
'YData', obj.motor(2,[1 3]), ...
'ZData', obj.motor(3,[1 3]));
set(tmpLine(1), 'Position', ...
[obj.motor(1,5) + obj.text_dist, ...
obj.motor(2,5) + obj.text_dist, ...
obj.motor(3,5) + obj.text_dist]);
end
end
% drawnow
pause(obj.drawspeed)
y = 1;
elseif b3dPlot && iter==0 && obj.k_int==1
obj.k_int = 0;
y = 0;
else
y = 0;
end
end
function resetImpl(obj)
% Initialize / reset discrete-state properties
nCol = obj.max_time/obj.cstep;
obj.time_hist = zeros(6, nCol);
obj.state_hist = zeros(6, nCol);
obj.state_des_hist = zeros(6, nCol);
obj.k = 0;
obj.k_int = 0;
end
end
%---------------------------------------------------------------------%
end
function [vOut, mRotEuler] = aux_rotEuler( vIn, eulerAngles, direction, convention)
% function [vOut, mRotEuler] = aux_rotEuler( eulerAngle, direction, convenction )
%
% Author1 : Eugen Nuss (e.nuss@irt.rwth-aachen.de)
%
% Last change : Spring 2019
%
% Description : Rotates the vector vIn by using euler angles with
% specified sequence to body or to inertial frame
%
% Parameters : vIn -> Vector that is to be rotated
% eulerAngles -> Vector containing euler angles
% direction -> Containts information whether rotate to
% "to-body-frame" or "to-inertial-frame"
% convention -> Containts information which rotation
% sequence to use (standard 'xyz')
%
% Return : vOut -> Rotated vector
% mRotEuler -> Euler rotation matrix
%
%-------------------------------------------------------------------------%
% Get euler angels
phi = eulerAngles(1);
theta = eulerAngles(2);
psi = eulerAngles(3);
% Set convention to 'xyz' if not specified
if nargin==3
convention = 'xyz';
end
% Calculate rotation matrix and output vector
switch convention
case 'xyz'
mRotEuler_toBody = ...
[cos(theta)*cos(psi) ...
cos(theta)*sin(psi) ...
-sin(theta); ...
sin(phi)*sin(theta)*cos(psi)-cos(phi)*sin(psi) ...
sin(phi)*sin(theta)*sin(psi)+cos(phi)*cos(psi) ...
sin(phi)*cos(theta); ...
cos(phi)*sin(theta)*cos(psi)+sin(phi)*sin(psi) ...
cos(phi)*sin(theta)*sin(psi)-sin(phi)*cos(psi) ...
cos(phi)*cos(theta)];
switch direction
case 'to-body-frame'
mRotEuler = mRotEuler_toBody;
vOut = mRotEuler * vIn;
case 'to-inertial-frame'
mRotEuler = mRotEuler_toBody.';
vOut = mRotEuler * vIn;
end
end
\ No newline at end of file
function Par = mdl_quadrotor_2D_par( )
% Par = mdl_quadrotor_2D_par( )
%
% Author1 :
%
% Last change : Spring 2019
%
% Description : Sets the parameters of a 2D quadrotor model
%
% Parameters : None
%
% Return : Par -> Struct containing the model parameters
%
%-------------------------------------------------------------------------%
clear all; close all; clc;
% Load quadrotor model parameters
ParQuad2D = mdl_quadrotor_2D_par();
% Load input trajectory
load('data_inputTrajectory.mat')
\ No newline at end of file
<?xml version="1.0" encoding="UTF-8"?>
<MF0 version="1.1" packageUris="http://schema.mathworks.com/mf0/SlCache/19700101">
<slcache.FileAttributes type="slcache.FileAttributes" uuid="375bcace-1ad8-464f-8728-3a7bef0ce3b2">
<checksum>K9zIHcqzZcOHV/NdzNTiZg==</checksum>
</slcache.FileAttributes>
</MF0>
\ No newline at end of file
classdef aux_quadPlot_simulink < matlab.System & handle & matlab.system.mixin.Propagates
% Visualization class for quadrotor simulation
%
% Author1 : Pennsylvania State University
% Author2 : Eugen Nuss (e.nuss@irt.rwth-aachen.de)
%
% Last change :
%
% Description : Visualization class for quadrotor simulation
%
% Attributes :
%
% Methods :
%
%-------------------------------------------------------------------------%
%---------------------------------------------------------------------%
% Set attributes %
%---------------------------------------------------------------------%
properties (Access = public)
qn = 1; % Quad number
color = [0 0.447 0.741]; % Color of quad
wingspan = 0.1; % Wingspan
cstep = 0.1; % Image capture time interval/ Update step width [s]
max_time = 50;
height = 0.04; % Height of quad
text_dist = 0.03; % Distance of quad number to quad
drawspeed = 0.02;
end
properties(DiscreteState)
state_hist % Position history
state_des_hist % Desired position history
time_hist % Time history
k % Counter
k_int % Counter if xk+1 = xk+T for T = 0 or T
end
% Pre-computed constants
properties(Access = private)
h_3d % Axis-handle of 3D plot
h_xz % Axis-handle of x-z-plane
h_yz % Axis-handle of y-z-plane
h_xy % Axis-handle of x-y-plane
h_xzBody % Axis-handle of body fixed x-z-plane
h_yzBody % Axis-handle of body fixed y-z-plane
motor; % Motor position
end
%---------------------------------------------------------------------%
%---------------------------------------------------------------------%
% Define methods %
%---------------------------------------------------------------------%
methods(Access = protected)
function [sz,dt,cp] = getDiscreteStateSpecificationImpl(obj,name)
switch name
case {'state_hist', 'state_des_hist', 'time_hist'}
nCol = obj.max_time/obj.cstep;
sz = [6 nCol];
dt = 'double';
cp = false;
case {'k', 'k_int'}
sz = [1 1];
dt = 'double';
cp = false;
otherwise
error(['Error: Incorrect State Name: ', name]);
end
end
function dt1 = getOutputDataTypeImpl(~)
dt1 = 'double';
end
function cp1 = isOutputComplexImpl(~)
cp1 = false;
end
function fz1 = isOutputFixedSizeImpl(~)
fz1 = true;
end
function num = getNumOutputsImpl(~)
num = 1;
end
function sze = getOutputSizeImpl(~)
sze = [1 1];
end
function setupImpl(obj, ~, state, stateDesired, ~)
% Perform one-time calculations, such as computing constants
close all;
disp('Initializing figures...');
figure;
sp3d = subplot(3,3,[1,2,4,5]);
obj.h_3d = hggroup;
axis equal
grid on
view(3);
xlabel('x [m]'); ylabel('y [m]'); zlabel('z [m]')
spxz = subplot(333);
obj.h_xz = hggroup;
axis equal
grid on
box on
view(0,0);
xlabel('x [m]'); ylabel('y [m]'); zlabel('z [m]')
spyz = subplot(336);
obj.h_yz = hggroup;
axis equal
grid on
box on
view(90,0);
xlabel('x [m]'); ylabel('y [m]'); zlabel('z [m]')
spxy = subplot(339);
obj.h_xy = hggroup;
axis equal
grid on
box on
view(0,90);
xlabel('x [m]'); ylabel('y [m]'); zlabel('z [m]')
spxzBody = subplot(337);
obj.h_xzBody = hggroup;
axis equal
grid on
box on
view(0,0);
xlabel('x [m]'); ylabel('y [m]'); zlabel('z [m]')
spyzBody = subplot(338);
obj.h_yzBody = hggroup;
axis equal
grid on
box on
view(90,0);
xlabel('x [m]'); ylabel('y [m]'); zlabel('z [m]')
UpdateQuadState(obj, state)
% Initialize 3D plot handle
groupHandles = {obj.h_xzBody, obj.h_yzBody, obj.h_3d, ...
obj.h_xz, obj.h_yz, obj.h_xy};
subPlots = {spxzBody, spyzBody, sp3d, spxz, spyz, spxy};
for ii = 1:length(groupHandles)
hold(subPlots{ii}, 'on')
if ii>2
plot3(state(1), state(2), state(3), 'r.', 'parent', groupHandles{ii});
plot3(stateDesired(1), stateDesired(2), stateDesired(3), 'b.', 'parent', groupHandles{ii});
end
plot3(obj.motor(1,[1 3]), ...
obj.motor(2,[1 3]), ...
obj.motor(3,[1 3]), ...
'-ko', 'MarkerFaceColor', obj.color, 'MarkerSize', 5, 'parent', groupHandles{ii});
plot3(...
obj.motor(1,[2 4]), ...
obj.motor(2,[2 4]), ...
obj.motor(3,[2 4]), ...
'-ko', 'MarkerFaceColor', obj.color, 'MarkerSize', 5, 'parent', groupHandles{ii});
plot3(...