classdef QuadPlot < handle
%QUADPLOT Visualization class for quad
properties (SetAccess = public)
k = 0;
qn; % quad number
time = 0; % time
state; % state
rot; % rotation matrix body to world
color; % color of quad
wingspan; % wingspan
height; % height of quad
motor; % motor position
state_hist; % position history
time_hist; % time history
max_iter; % max iteration
end
properties (SetAccess = private)
h_3d
h_m13; % motor 1 and 3 handle
h_m24; % motor 2 and 4 handle
h_qz; % z axis of quad handle
h_qn; % quad number handle
h_pos_hist; % position history handle
text_dist; % distance of quad number to quad
end
methods
% Constructor
function Q = QuadPlot(qn, state, wingspan, height, color, max_iter, h_3d)
Q.qn = qn;
Q.state = state;
Q.wingspan = wingspan;
Q.color = color;
Q.height = height;
Q.rot = QuatToRot(Q.state(7:10));
Q.motor = quad_pos(Q.state(1:3), Q.rot, Q.wingspan, Q.height);
Q.text_dist = Q.wingspan / 3;
Q.max_iter = max_iter;
Q.state_hist = zeros(6, max_iter);
Q.time_hist = zeros(1, max_iter);
% Initialize plot handle
if nargin < 7, h_3d = gca; end
Q.h_3d = h_3d;
hold(Q.h_3d, 'on')
Q.h_pos_hist = plot3(Q.h_3d, Q.state(1), Q.state(2), Q.state(3), 'r.');
Q.h_m13 = plot3(Q.h_3d, ...
Q.motor(1,[1 3]), ...
Q.motor(2,[1 3]), ...
Q.motor(3,[1 3]), ...
'-ko', 'MarkerFaceColor', Q.color, 'MarkerSize', 5);
Q.h_m24 = plot3(Q.h_3d, ...
Q.motor(1,[2 4]), ...
Q.motor(2,[2 4]), ...
Q.motor(3,[2 4]), ...
'-ko', 'MarkerFaceColor', Q.color, 'MarkerSize', 5);
Q.h_qz = plot3(Q.h_3d, ...
Q.motor(1,[5 6]), ...
Q.motor(2,[5 6]), ...
Q.motor(3,[5 6]), ...
'Color', Q.color, 'LineWidth', 2);
Q.h_qn = text(...
Q.motor(1,5) + Q.text_dist, ...
Q.motor(2,5) + Q.text_dist, ...
Q.motor(3,5) + Q.text_dist, num2str(qn));
hold(Q.h_3d, 'off')
end
% Update quad state
function UpdateQuadState(Q, state, time)
Q.state = state;
Q.time = time;
Q.rot = QuatToRot(state(7:10))'; % Q.rot needs to be body-to-world
end
% Update quad history
function UpdateQuadHist(Q)
Q.k = Q.k + 1;
Q.time_hist(Q.k) = Q.time;
Q.state_hist(:,Q.k) = Q.state(1:6);
end
% Update motor position
function UpdateMotorPos(Q)
Q.motor = quad_pos(Q.state(1:3), Q.rot, Q.wingspan, Q.height);
end
% Truncate history
function TruncateHist(Q)
Q.time_hist = Q.time_hist(1:Q.k);
Q.state_hist = Q.state_hist(:, 1:Q.k);
end
% Update quad plot
function UpdateQuadPlot(Q, state, time)
Q.UpdateQuadState(state, time);
Q.UpdateQuadHist();
Q.UpdateMotorPos();
set(Q.h_m13, ...
'XData', Q.motor(1,[1 3]), ...
'YData', Q.motor(2,[1 3]), ...
'ZData', Q.motor(3,[1 3]));
set(Q.h_m24, ...
'XData', Q.motor(1,[2 4]), ...
'YData', Q.motor(2,[2 4]), ...
'ZData', Q.motor(3,[2 4]));
set(Q.h_qz, ...
'XData', Q.motor(1,[5 6]), ...
'YData', Q.motor(2,[5 6]), ...
'ZData', Q.motor(3,[5 6]))
set(Q.h_qn, 'Position', ...
[Q.motor(1,5) + Q.text_dist, ...
Q.motor(2,5) + Q.text_dist, ...
Q.motor(3,5) + Q.text_dist]);
set(Q.h_pos_hist, ...
'XData', Q.state_hist(1,1:Q.k), ...
'YData', Q.state_hist(2,1:Q.k), ...
'ZData', Q.state_hist(3,1:Q.k));
drawnow;
end
end
end
没有合适的资源?快使用搜索试试~ 我知道了~
matlab-基于模糊控制器的UAV行驶轨迹控制matlab仿真-源码
共12个文件
m:11个
avi:1个
1.该资源内容由用户上传,如若侵权请联系客服进行举报
2.虚拟产品一经售出概不退款(资源遇到问题,请及时私信上传者)
2.虚拟产品一经售出概不退款(资源遇到问题,请及时私信上传者)
版权申诉
0 下载量 8 浏览量
2021-09-20
02:35:01
上传
评论
收藏 281KB RAR 举报
温馨提示
matlab_基于模糊控制器的UAV行驶轨迹控制matlab仿真_源码
资源推荐
资源详情
资源评论
收起资源包目录
matlab_基于模糊控制器的UAV行驶轨迹控制matlab仿真_源码.rar (12个子文件)
matlab_基于模糊控制器的UAV行驶轨迹控制matlab仿真_源码
utils
sys_params.m 226B
QuatToRot.m 284B
quad_pos.m 514B
simStateToQuadState.m 630B
QuadPlot.m 4KB
sys_eom.m 1KB
trajectories
simulation_2d.m 4KB
traj_line.m 643B
controller.m 1KB
traj_sine.m 953B
Runme.m 266B
test_control_2d.avi 94.81MB
共 12 条
- 1
资源评论
mYlEaVeiSmVp
- 粉丝: 1924
- 资源: 19万+
上传资源 快速赚钱
- 我的内容管理 展开
- 我的资源 快来上传第一个资源
- 我的收益 登录查看自己的收益
- 我的积分 登录查看自己的积分
- 我的C币 登录后查看C币余额
- 我的收藏
- 我的下载
- 下载帮助
安全验证
文档复制为VIP权益,开通VIP直接复制
信息提交成功