简单有趣的小游戏(生活中简单有趣的事)

来源:https://github.com/aerojunkie/control-tools/blob/master/ufo_rotate.m一个简单的LQR例子closeall%InitialConditionsx0=[3;%3radians0];%0rad/s%SystemDynamicsA=[01;0.010];B=[0;1];C=[10];D=0;%ControlLaw

大家好,又见面了,我是你们的朋友全栈君。

来源:https://github.com/aerojunkie/control-tools/blob/master/ufo_rotate.m

一个简单的LQR例子
在这里插入图片描述

close all

% Initial Conditions
x0 = [3;  % 3 radians
      0]; % 0 rad/s

% System Dynamics
A = [0    1; 
     0.01 0];
B = [0; 
     1];
C = [1 0];
D = 0;

% Control Law
Q = [1 0;  % Penalize angular error
     0 1]; % Penalize angular rate
R = 1;     % Penalize thruster effort
K = lqr(A,B,Q,R);

% Closed loop system
sys = ss((A - B*K), B, C, D);

% Run response to initial condition
t = 0:0.005:30;
[y,t,x] = initial(sys, x0, t);

%% Everything below this is just for the visualization

% Patch drawings
ufo_data = [
    0.54 .12;
    0.48 0.24;
    0.42 0.31;
    0.3 0.4;
    0.18 0.45;
    0.06 0.48;
    -0.06 0.48;
    -0.18 0.45;
    -0.3 0.4;
    -0.42 0.31;
    -0.48 0.24;
    -0.54 .12;
    0.54 .12;
    0.54 0.06;
    0.6 0.06;
    0.78 0.03; 
    0.91 0.01;
    0.91 0.03;
    1.01 0.03;
    1.01 -0.01;   
    1.1 -0.03; 
    1.26 -0.12; 
    1.36 -0.18; 
    1.40 -0.21;
    1.38 -0.24;
    1.26 -0.27;
    1.1 -0.3;  
    1.01 -0.32;
    1.01 -0.34;
    0.91 -0.34;
    0.91 -0.32;  
    0.72 -0.34;
    0.48 -0.35;
    0.72 -0.6;
    0.62 -0.6;
    0.38 -0.35;    
    0.3 -0.35;    
    -0.3 -0.35;
    -0.38 -0.35;
    -0.62 -0.6;
    -0.72 -0.6;
    -0.48 -0.35;
    -0.91 -0.32;
    -0.91 -0.34;
    -1.01 -0.34;
    -1.01 -0.32;
    -1.1 -0.3;
    -1.26 -0.27;
    -1.38 -0.24;
    -1.40 -0.21;
    -1.36 -0.18;
    -1.26 -0.12;
    -1.1 -0.03;
    -1.01 -0.01;
    -1.01 0.03;
    -0.91 0.03;
    -0.91 0.01;
    -0.78 0.03;
    -0.6 0.06;
    -0.54 0.06;
    0.54 0.06;
];

thrust = [
    0.05 0;
    -.05 0;
    -.06 -0.1;
    -.05 -0.1;
    -.06 -0.2;
    0 -0.25;
    0.06 -0.2;
    0.05 -0.1;
    0.06 -0.1;
    0.05 0];

% yoffset moves the patch up from 0, 0
yoffset = 1;

%% Create Animation

% Rotate to inital condition
ufo_rot = rot(ufo_data, x0(1));
r_thrust_rot = rot(thrust, x0(1));
l_thrust_rot = rot(thrust, x0(1));

% Set up the figure and the subplots
fig = figure('Position', [100 80 670 800]);
h3 = subplot(3, 1, 3);
h3.OuterPosition = [0 0.01 1 .19];
h3a = area(0, 0);
hold on
h3p = plot(0, 0, 'r');
h3.YGrid = 'on';
h3.XGrid = 'on';
h3.LineWidth = 1;
h3.Color = [0 0 0];
h3p.LineWidth = 4;
axis([0 12 -3 3]);
h3.GridColor = 'w';
h3.GridAlpha = 0.5;
text(6.1, 1.5, 'Acceleration (fuel)', 'Color', [1 0 0], 'FontSize', 24)

h2 = subplot(3, 1, 2);
h2.OuterPosition = [0 .2 1 .2];
h2p = plot(0, 0, 'g');
h2p.LineWidth = 4;
h2.YGrid = 'on';
h2.XGrid = 'on';
h2.LineWidth = 1;
h2.GridColor = 'w';
h2.GridAlpha = 0.5;
axis([0 12 -.5 3.5]);
h2.Color = [0 0 0];
text(6.1, 2.5, 'Angular Error (time)', 'Color', [0 1 0], 'FontSize', 24)

h1 = subplot(3, 1, 1);
h1.OuterPosition = [0 .4 1 0.63];
h1.XTick = [];
h1.YTick = [];
patch([-3 3 3 -3], [-1.7 -1.7 3.3 3.3], 'k');
axis([-3 3 -1.7 3.3]);
line([0 0.28], [1 2.99], 'Color', [0.5 0.3 0.3], 'LineWidth', 1);
line([0 0], [1 -1], 'Color', [0.3 0.5 0.3], 'LineWidth', 1);
H = patch(ufo_rot(:, 1), ufo_rot(:, 2) + yoffset, [0.7 0.7 0.7]);
RBT = patch(r_thrust_rot(:, 1), r_thrust_rot(:, 2), 'r');
LBT = patch(l_thrust_rot(:, 1), l_thrust_rot(:, 2), 'r');
RTT = patch(r_thrust_rot(:, 1), r_thrust_rot(:, 2), 'r');
LTT = patch(l_thrust_rot(:, 1), l_thrust_rot(:, 2), 'r');
RBT.FaceAlpha = 0;
LBT.FaceAlpha = 0;
RTT.FaceAlpha = 0;
LTT.FaceAlpha = 0;
RBT.EdgeColor = 'none';
LBT.EdgeColor = 'none';
RTT.EdgeColor = 'none';
LTT.EdgeColor = 'none';
hold on

% Initial time and fuel values
current_time = 0;
fuel = 0;

% Write time and fuel to the screen
text(-2.5, 3, 'Time: sec', 'Color', [1 1 1], 'FontSize', 24,...
    'HorizontalAlignment', 'left');
text(-2.5, 2.7, 'Fuel: units', 'Color', [1 1 1], 'FontSize', 24,...
    'HorizontalAlignment', 'left');

TME = text(-1.1, 3, num2str(current_time, '%.1f'), 'Color', [0 1 0], ...
'FontSize', 24, 'HorizontalAlignment', 'right');
FUL = text(-1.1, 2.7, num2str(fuel, '%.0f'), 'Color', [1 0 0], ...
'FontSize', 24, 'HorizontalAlignment', 'right');

accel_true = diff(x(:, 2))/(t(2)-t(1));

pause(2)

% Update Animation

for i = 1:5:length(x)-1
    
    % Simulation ending condition
    if and(abs(x(i,1)) < 0.011, abs(x(i,2)) < 0.011)
        x(i,1) = 0;
        pause(1);
        % Turn on tractor beam
        G = patch([0.1 0.8 -0.8 -0.1], [-0.35 -2.3 -2.3 -0.35] + yoffset, 'y');
        G.FaceAlpha = 0.6;
        return
    end
    
    % Scale acceleration to make thruster size look better fuel units
    % just a few digits (asthetics)
    accel_scaled = (x(i+1, 2) - x(i, 2));
    
    fuel = fuel + abs(accel_scaled);
    
    accel_scaled = min(2, max(-2, accel_scaled * 1000));
    
    % Scale thruster based on accleration and direction
    if accel_scaled > 0
        r_scaled(:, 1) = thrust(:, 1) * accel_scaled * 0.5;
        r_scaled(:, 2) = thrust(:, 2) * accel_scaled;
        l_scaled(:, 1) = thrust(:, 1);
        l_scaled(:, 2) = thrust(:, 2);
        
    elseif accel_scaled < 0
        r_scaled(:, 1) = thrust(:, 1);
        r_scaled(:, 2) = thrust(:, 2);
        l_scaled(:, 1) = thrust(:, 1) * -accel_scaled * 0.5;
        l_scaled(:, 2) = thrust(:, 2) * -accel_scaled;
    else
        r_scaled(:, 1) = thrust(:, 1);
        r_scaled(:, 2) = thrust(:, 2);
        l_scaled(:, 1) = thrust(:, 1);
        l_scaled(:, 2) = thrust(:, 2);
    end
    
    % Offset thrusters to place correctly on UFO
    rb_thrust = r_scaled + [0.96 -0.34];
    lb_thrust = l_scaled + [-0.96 -0.34];
    rt_thrust = l_scaled * [1 0; 0 -1] + [0.96, 0.03];
    lt_thrust = r_scaled * [1 0; 0 -1] + [-0.96, 0.03];
    
    % Rotate thrusters with UFO
    rb_thrust_rot = rot(rb_thrust, x(i,1)) + [0 yoffset];
    lb_thrust_rot = rot(lb_thrust, x(i,1)) + [0 yoffset];
    rt_thrust_rot = rot(rt_thrust, x(i,1)) + [0 yoffset];
    lt_thrust_rot = rot(lt_thrust, x(i,1)) + [0 yoffset];
    
    % Update thruster data 
    RBT.XData = rb_thrust_rot(:, 1);
    RBT.YData = rb_thrust_rot(:, 2);
    LBT.XData = lb_thrust_rot(:, 1);
    LBT.YData = lb_thrust_rot(:, 2);
    RTT.XData = rt_thrust_rot(:, 1);
    RTT.YData = rt_thrust_rot(:, 2);
    LTT.XData = lt_thrust_rot(:, 1);
    LTT.YData = lt_thrust_rot(:, 2);
    
   
    % Turn thrusters off if acceleration is low 
    if accel_scaled > 2e-1
        RBT.FaceAlpha = 1;
        LBT.FaceAlpha = 0;
        RTT.FaceAlpha = 0;
        LTT.FaceAlpha = 1;
        
    elseif accel_scaled < -2e-1
        RBT.FaceAlpha = 0;
        LBT.FaceAlpha = 1;
        RTT.FaceAlpha = 1;
        LTT.FaceAlpha = 0;
    else
        RBT.FaceAlpha = 0;
        LBT.FaceAlpha = 0;
        RTT.FaceAlpha = 0;
        LTT.FaceAlpha = 0;
    end
    
    % Rotate ufo
    ufo_rot = rot(ufo_data, x(i,1));
    H.XData = ufo_rot(:, 1);
    H.YData = ufo_rot(:, 2) + yoffset;
    h2p.XData = t(1:i);
    h2p.YData = x(1:i, 1);
    h3p.XData = t(1:i);
    h3p.YData = accel_true(1:i);
    h3a.XData = t(1:i);
    h3a.YData = accel_true(1:i);
    
    current_time = num2str(t(i), '%.1f');

    % Update time and fuel
    TME.String = current_time;
    FUL.String = num2str(fuel*30, '%.0f');
    
    pause(0.0001);
end

function output = rot(data, angle)
output = size(data);
Q = [cos(angle) -sin(angle); sin(angle) cos(angle)];
for i = 1:length(data)
    output(i, :) = Q * data(i, :)';
end
end
版权声明:本文内容由互联网用户自发贡献,该文观点仅代表作者本人。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如发现本站有涉嫌侵权/违法违规的内容, 请发送邮件至 举报,一经查实,本站将立刻删除。

发布者:全栈程序员-用户IM,转载请注明出处:https://javaforall.cn/127941.html原文链接:https://javaforall.cn

【正版授权,激活自己账号】: Jetbrains全家桶Ide使用,1年售后保障,每天仅需1毛

【官方授权 正版激活】: 官方授权 正版激活 支持Jetbrains家族下所有IDE 使用个人JB账号...

(0)


相关推荐

  • pycharm企业版激活码[最新免费获取]

    (pycharm企业版激活码)本文适用于JetBrains家族所有ide,包括IntelliJidea,phpstorm,webstorm,pycharm,datagrip等。https://javaforall.cn/100143.htmlIntelliJ2021最新激活注册码,破解教程可免费永久激活,亲测有效,上面是详细链接哦~S…

  • 自体脂肪填充的风险_numpy 插值

    自体脂肪填充的风险_numpy 插值一、接口pad(array,pad_width,mode,**kwargs)其中,第一个参数是输入数组;第二个参数是需要pad的值,参数输入方式为:((before_1,after_1),…(before_N,after_N)),其中(before_1,after_1)表示第1轴两边缘分别填充before_1个和after_1个数值;第三个参数是pad模式‘constant’——表示连续填充相同的值,每个轴可以分别指定填充值,constant_values=(x,y

  • 是学Java好呢?还是学C++更有前途?

    是学Java好呢?还是学C++更有前途?最近有不少初学编程的朋友问:他们比较倾向于Java和C++作为他们首选学习语言,但是学Java好呢?还是学C++更有前途?到底哪一门语言更有“钱途”呢?这个问题问的好,很多初学者都会有类似的疑问,今天我就来给大家简单的解答一下。1、C++和Java在编程排行榜中的排名作为IT行业的软件研发人员,在我看来,这两种语言都可以学,都很有前途。其实Java和C++一直名列世界编程语言排行榜的前几位,始终是热门的开发语言,下图是最近的世界编程语言排行榜:不管是从世界…

  • 差分进化算法之Matlab实现「建议收藏」

    差分进化算法之Matlab实现「建议收藏」一、介绍差分进化算法是模拟自然界生物种群以“优胜劣汰,适者生存”为原则的进化发展规律而形成的一种随机启发式搜索算法。其保留了基于种群的全局搜索策略,采用实数编码,基于差分的简单变异操作和一对一的竞争生存策略,比遗传算法更简单。同时,差分进化算法独特的记忆能力使其可以动态的跟踪当前的搜索情况,及时调整搜索测量,因此具有较强的全局收敛能力。目前为止,差分进化算法已经成为一种求解非线性,不可微,多极…

    2022年10月24日
  • ssh 命令连接

    ssh 命令连接Linuxssh命令详解SSH(远程连接工具)连接原理:ssh服务是一个守护进程(demon),系统后台监听客户端的连接,ssh服务端的进程名为sshd,负责实时监听客户端的请求(IP22端口),包括公共秘钥等交换等信息。ssh服务端由2部分组成:openssh(提供ssh服务)openssl(提供加密的程序)ssh的客户端可以用XSHELL,Securecrt,Moba…

  • Spring学习—生成图片验证码

    今天想学下一下验证码的生成,就之前搭建好的一个spring框架上写了一个demo,我会贴出细节代码,但是spring的配置就不在介绍了。需要完整代码可以联系我! 会从前台页面到后台实现完整的讲解: 1:前台的代码,image.jsp<%@ page language="java" contentType="text/html; charset=UTF-8" pageEncoding="U

发表回复

您的电子邮箱地址不会被公开。

关注全栈程序员社区公众号