北航惯性导航综合实验四实验报告.

基于运动规划的惯性导航系统动态实验

二零一三年六月十日

实验4.1 惯性导航系统运动轨迹规划与设计实验

一、实验目的

为进行动态下简化惯性导航算法的实验研究，进行路径和运动状态规划，以验证不同运动状态下惯导系统的性能。通过实验掌握步进电机控制方法，并产生不同运动路径和运动状态。

二、实验内容

学习利用6045B 控制板对步进电机进行控制的方法，并控制电机使运动滑轨产生定长运动和不同加速度下的定长运动。

三、实验系统组成

USB_PCL6045B 控制板（评估板）、运动滑轨和控制计算机组成。

四、实验原理

IMU安装误差系数的计算方法

USB_PCL6045B 控制板采用了USB 串行总线接口通信方式，不必拆卸计算机箱就可以在台式机或笔记本电脑上进行运动控制芯片PCL6045B 的学习和评估。

USB_PCL6045B 评估板采用USB 串行总线方式实现评估板同计算机的数据交换，由评估板的FIFO 控制回路完成步进电机以及伺服电机的高速脉冲控制，任意 2 轴的圆弧插补，2-4 轴的直线插补等运动控制功能。USB_PCL6045B 评估板上配置了全部PCL6045B 芯片的外部信号接口和增量编码器信号输入接口。由

USB_PCL6045B 评估测试软件可以进行PCL6045B 芯片的主要功能的评估测试。

图4-1-1USB_PCL6045B 评估板原理框图

如图4-1-1 所示，CN11 接口主要用于外部电源连接，可以选择DC5V 单一电源或DC5V/24V 电源。CN12 接口是USB 信号接口，用于USB_PCL6045B 评估板同计算机的数据交换。

USB_PCL6045B 评估板已经完成对PCL6045B 芯片的底层程序开发和硬件资源与端口的驱动，并封装成156 个API 接口函数。用户可直接在VC 环境下利用API 接口函数进行编程。

五、实验内容

1、操作步骤

1）检查电机驱动电源（24V）

2）检查USB_PCL6045B 控制板与上位机及电机驱动器间的连接电缆

3）启动USB_PCL6045B 控制板评估测试系统检查系统是否正常工作。

4）运行编写的定长运动程序，并比较实际位移与设定位移。

5）修改程序设定不同运动长度，并重复执行步骤4）。

6）对记录实验数据，并进行误差分析。

2、实验数据处理

基于VC的控制界面：

本次实验必须先设计控制系统的上位机，通过上位机的串口向下位机发送控制命令，下位机接收到命令后，产生PWM波，控制电机的正反转以此达到控制导轨运动的目的。系统的控制界面如图1所示：

图1 系统的控制界面

控制导轨运动，运动采取正向运动，再返回，即IMU的实际运行位移为零。并保存数据

控制界面的应用程序

源程序仅写出VC中按钮的响应程序：

void CAaaDlg::Online() ////定长运动

{

// TODO: Add your control notification handler code here

USB_initial();

USB_default_set();

USB_set_org_logic(AXS_AX,0);//原点开关的逻辑, 负逻辑

USB_set_el_logic(AXS_AX,0);//硬极限输入逻辑,低电平使能

USB_set_sd_logic(AXS_AX,0);//减速开关的输入逻辑,负逻辑

USB_set_alm_logic(AXS_AX, 1);//报警输入信号逻辑

USB_set_inp_logic(AXS_AX,1);//in的输入信号逻辑

USB_ez_logic(AXS_AX,0);//Z相的输入逻辑

USB_set_pls_outmode(AXS_AX,1);

USB_set_out_enable(AXS_AX,1);//脉冲输出使能

// USB_jog_continue(AXS_AX,150,20000,20,20,20,20,1,30000);

USB_start_tr_move(AXS_AX, m_dist, 0, m_inspeed, 5000, 5000); // USB_tv_move(AXS_AX, 150, 2000, 3000);

/* USB_v_change(AXS_AX, 5000, 5000);

while(1)

{

USB_get_speed(AXS_AX, &m_speed);

UpdateData(FALSE);

MSG msg;

while(PeekMessage(&msg,0,0,100,PM_REMOVE))

{

TranslateMessage(&msg);

DispatchMessage(&msg);

}

Sleep(100);

}

*/

}

void CAaaDlg::OnButton1() //////停止运动

{

// TODO: Add your control notification handler code here

USB_sd_stop(AXS_AX);

}

void CAaaDlg::OnGetSpeed() //////获得速度

{

// TODO: Add your control notification handler code here

USB_get_speed(AXS_AX, &m_speed);

UpdateData(FALSE);

}

void CAaaDlg::OnButton3() ///OK 按钮程序

{

// TODO: Add your control notification handler code here

UpdateData(true);

}

3，处理数据

由实验原理可知，惯性测量单元（IMU）可以通过自身独立的测量结果进行积分，计算出目标运动的角度和位移等量。本次实验就是利用IMU的加速度计的某一敏感轴测量导轨运行的加速度，通过加速度两次积分得到物体的位移，计算结果如图2所示：

实验经过往返，从原理上讲位移应该为零。

处理结果：

位移曲线：

速度曲线：

4，源程序：

A=load('E:\惯性器件综合实验\我的作业\实验四\X300000_V10000.txt');

T=1/200; %%%%单位为秒

g=9.78;

Ax=A(:,4)*g/1000; %%%提取加速度计的值转化为m/s^2

Ax=Ax*(1.0009)-0.0036595*g;

vx=zeros(12657,1);

sx=zeros(12657,1);

u=zeros(12657,1);

%%%%%计算位移

for i=2:12657

vx(i)=vx(i-1)+(Ax(i-1)+Ax(i-1))*T /2;

sx(i)=sx(i-1)+(vx(i-1)+ vx(i))/2*T+0.5*A(i-1)*T*T;

u(i)=i;

end

figure

plot(u/100,vx);

xlabel('时间/秒'),ylabel('速度米/秒');

figure

plot(u/100,sx);

xlabel('时间/秒'),ylabel('位移米');

5，实验结果分析

从原理上讲IMU做往返运动，位移应该出现增大和减小的趋势，但是由于各种误差角，而且滑轨也不能保证当地水平，在计算过程中，也未减去有害加速度。所以误差很大。而且根据所采集的数据可知加速度计并没有感知方向，在实验过程中应该根据计算脉冲与时间，自己计算方向时间

惯性导航系统半物理仿真实验

一、实验目的

进行惯导系统半物理仿真实验，以验证惯性器件真实误差特性情况下惯性导航系统的性能。

二、实验内容

将采集到陀螺仪与加速度计的真实误差数据叠加到轨迹发生器产生的导航参数真值上，进行惯导解算，并分析误差特性。

三、实验系统组成

真实的陀螺仪、加速度计或 IMU，数据采集系统和数据处理计算机。

四、实验步骤

（1）采集实验数据

（2）处理采集的实验数据，生成半物理的惯性器件误差数据

（3）生成半物理的导航数据，进行导航解算

（4）对导航解算结果进行分析

（5）完成实验报告

五、实验内容及结果

（1）半物理仿真数据的生成：

a)应用前面IMU实验或惯性导航系统动态实验中采集的陀螺仪与加速度计的

静态数据DATA

b)对以上采集的静态数据求取均方差X（结果为X度/小时或Xg）

c)将DATA中数据去掉均值生成新的数据DATA1（器件噪声）

d)自己设定要仿真的陀螺或加速度计的精度Y（度/小时或g）

e)将DATA1中数据乘以Y/X生成新的数据DATA2（半物理仿真噪声）

f)从DATA2中读取数据并叠加到轨迹发生器产生的标准数据(不含噪声)上，

进行导航解算。（如初始采集的数据长度不够，可以将DATA2中数据重复利用，即将生成一个几倍长度于DATA2和数据文件DATA3，并从DATA3中读取半物理数据并叠加到轨迹发生器产生的标准数据上）

（2）加半物理仿真噪声数据的导航结果：

（3）叠加噪声的导航结果：

（4）结果分析：

由实验结果可见，叠加的仿真噪声数据对姿态的解算影响很大；但由于所加噪声较小，所以噪声数据对位移和速度的解算影响不大。

六，源程序

clear,clc

invout=load('E:\惯导实验数据\第四次\实验4.3\第四部分半物理仿真数据生成方法及数据格式说明\IMU数据\invout.dat');

CaijiShuju=load('E:\惯导实验数据\第四次\实验4.3\第四部分半物理仿真数据生成方法及数据格式说明\IMU数据\data2.txt');

W=CaijiShuju(:,3:5);

F=CaijiShuju(:,9:11);

W_pingjun=mean(W);

F_pingjun=mean(F);

%%%%%%%%%%%%%%%%%%%%%%%%%%生成噪声%%%%%%%%%%%%%%

wx=W(:,1)-W_pingjun(1,1);%器件噪声

wy=W(:,2)-W_pingjun(1,2);

wz=W(:,3)-W_pingjun(1,3);

fx=F(:,1)-F_pingjun(1,1);

fx=F(:,2)-F_pingjun(1,2);

fx=F(:,3)-F_pingjun(1,3); %%%%%%%%%%%%%%%%%%%%%%%%%%%求陀螺均方差%%%%%%%%%%%%%%%%%%%%

N=size(W);

n=N(1,1);

%%%%%%%%%%%%%陀螺的精度设为0.5度/小时%%%%%%%%%%%%%%%%%%%

w_jingdu=0.5/3600*pi/180%0.5度/小时转成弧度

sx=0;

for i=1:n

sx=sx+(W(i,1)-W_pingjun(1,1))^2

end

wx_junfangcha=sqrt(sx/n);%x陀螺的均方差

wx1=w_jingdu/wx_junfangcha;

Wx=wx*wx1 %半物理仿真噪声Wx

sx=0;

for i=1:n

sx=sx+(W(i,2)-W_pingjun(1,2))^2

end

wy_junfangcha=sqrt(sx/n);%y陀螺的均方差

wy2=w_jingdu/wy_junfangcha;

Wy=wy*wy2 %半物理仿真噪声Wy

sx=0;

for i=1:n

sx=sx+(W(i,3)-W_pingjun(1,3))^2

end

wz_junfangcha=sqrt(sx/n);%z陀螺的均方差

wz3=w_jingdu/wz_junfangcha;

Wz=wz*wz3 %半物理仿真噪声Wz %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%求加计的均方差%%%%%%%%%%%%%%%%%%%%%%%

f_jingdu=1/1000*9.8%加计的精度为1mg

sx=0;

for i=1:n

sx=sx+(F(i,1)-F_pingjun(1,1))^2

end

fx_junfangcha=sqrt(sx/n);%x加计的均方差

fx1= f_jingdu/fx_junfangcha;

Fx=fx*fx1;

sx=0;

for i=1:n

sx=sx+(F(i,2)-F_pingjun(1,2))^2

end

fy_junfangcha=sqrt(sx/n);%y加计的均方差

fx2= f_jingdu/fy_junfangcha;

Fy=fx*fx2;

sx=0;

for i=1:n

sx=sx+(F(i,3)-F_pingjun(1,3))^2

end

fz_junfangcha=sqrt(sx/n);%z加计的均方差

fx3= f_jingdu/fz_junfangcha;

Fz=fx*fx3;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%轨迹发生器数据叠加噪声%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Wx_invout=invout(:,5);

Wy_invout=invout(:,6);

Wz_invout=invout(:,7);

Fx_invout=invout(:,2);

Fy_invout=invout(:,3);

Fz_invout=invout(:,4);

L_invout=invout(:,8); %纬度

Jingdu_invout=invout(:,9);%经度

Height_invout=invout(:,10);%高度

%%%%%%%%%%%%%%%开始叠加%%%%%%%%%%%%%%%%%%%%

N1=size(invout);

n1=N1(1,1);%%%%应为采样点数大于轨迹发生器的个数，所以以轨迹发生器的个数为准

Wxx=Wx(1:n1,1)+Wx_invout;

Wyy=Wy(1:n1,1)+Wy_invout ;

Wzz=Wz(1:n1,1)+Wz_invout;

Wibb=[Wxx,Wyy,Wzz];

Fxx=Fx(1:n1,1)+Fx_invout;

Fyy=Fy(1:n1,1)+Fy_invout ;

Fzz=Fz(1:n1,1)+Fz_invout;

Fibb=[Fxx,Fyy,Fzz];

q0=zeros(n1,1);

q1=zeros(n1,1);

q2=zeros(n1,1);

q3=zeros(n1,1);

Phai=zeros(n1,1);

Thita=zeros(n1,1);

Gama=zeros(n1,1);

Phai(1)=0/180*pi;%偏航初始角

Thita(1)=(0)*pi/180;%俯仰初始角

Gama(1)=(0)*pi/180;%横滚初始角

L=zeros(n1,1);

nmda=zeros(n1,1);

Vxt=zeros(n1+1,1);

Vyt=zeros(n1+1,1);

q0(1)=cos(-Phai(1)/2)*cos(Thita(1)/2)*cos(Gama(1)/2)+sin(-Phai(1)/2)*sin(Thita(1)/2)*sin(Gama(1)/2);

q1(1)=cos(-Phai(1)/2)*sin(Thita(1)/2)*cos(Gama(1)/2)+sin(-Phai(1)/2)*cos(Thita(1)/2)*sin(Gama(1)/2);

q2(1)=cos(-Phai(1)/2)*cos(Thita(1)/2)*sin(Gama(1)/2)-sin(-Phai(1)/2)*sin(Thita(1)/2)*cos(Gama(1)/2);

q3(1)=cos(-Phai(1)/2)*sin(Thita(1)/2)*sin(Gama(1)/2)-sin(-Phai(1)/2)*cos(Thita(1)/2)*cos(Gama(1)/2);

Wie=0.000072921151467;%已经是弧度制

L(1)=40/180*pi;

nmda(1)=116.0/180*pi;

T=0.01;%采样频率为100Hz

Vxt(1)=0;

Vyt(1)=0;

Re=6378245+80;%加高度80米

e=1/298.3;

for k=1:n1

c11=q0(k)^2+q1(k)^2-q2(k)^2-q3(k)^2;

c12=2*(q1(k)*q2(k)+q0(k)*q3(k));

c13=2*(q1(k)*q3(k)-q0(k)*q2(k));

c21=2*(q1(k)*q2(k)-q0(k)*q3(k));

c22=q0(k)^2-q1(k)^2+q2(k)^2-q3(k)^2;

c23=2*(q2(k)*q3(k)+q0(k)*q1(k));

c31=2*(q1(k)*q3(k)+q0(k)*q2(k));

c32=2*(q2(k)*q3(k)-q0(k)*q1(k));

c33=q0(k)^2-q1(k)^2-q2(k)^2+q3(k)^2;

Cnb=[c11,c12,c13

c21,c22,c23

c31,c32,c33];

if abs(c22)>0.0000000000001

Phai(k)=atan(-c21/c22);

end

if abs(c22)>0.0000000000001 & c21>0

Phai(k)=pi/2;

end

if abs(c22)>0.0000000000001 & c21<0

Phai(k)=-pi/2;

end

if abs(c22)>0.0000000000001 & c22>0

Phai(k)=atan(-c21/c22);

end

if abs(c22)>0.0000000000001 & c22>0 & c21>0

Phai(k)=atan(c21/c22)+pi;

end

if abs(c22)>0.0000000000001 & c22>0 & c21<0

Phai(k)=atan(-c21/c22)-pi;

end

Thita(k)=asin(c23);

Gama(k)=-atan(c13/c33);

Cbn=inv(Cnb);

Aibn=Cbn*Fibb(k,:)';

Rxt=Re/(1-e*(sin(L(k))*sin(L(k))));

axt=Aibn(1,1)+2*Wie*sin(L(k))*Vyt(k)+Vyt(k)*Vxt(k)*tan(L(k))/Rxt; ayt=Aibn(2,1)-2*Wie*sin(L(k))*Vxt(k)-Vxt(k)*Vxt(k)*tan(L(k))/Rxt;

Vxt(k+1)=axt*T+Vxt(k);

Vyt(k+1)=ayt*T+Vyt(k);

Ryt=Re/(1+2*e-3*e*(sin(L(k))*sin(L(k))));

L(k+1)=0.5*T*(Vyt(k+1)+Vyt(k))/Ryt+L(k);

nmda(k+1)=0.5*T*(Vxt(k+1)+Vxt(k))/Rxt*sec(L(k))+nmda(k);

Wenn=[-Vyt(k)/Ryt;Vxt(k)/Rxt;Vxt(k)/Rxt*tan(L(k))];%课本86页4.2-38式

Winn=Wenn+[0;Wie*cos(L(k));Wie*sin(L(k))];

Winb=Cnb*Winn;

Wtbb=Wibb(k,:)'-Winb;

dltaTita0_fang=(Wtbb(1,1)*T)^2+(Wtbb(2,1)*T)^2+(Wtbb(3,1)*T)^2;

dltaTita=[0,-Wtbb(1,1)*T,-Wtbb(2,1)*T,-Wtbb(3,1)*T;

Wtbb(1,1)*T,0,Wtbb(3,1)*T,-Wtbb(2,1)*T;

Wtbb(2,1)*T,-Wtbb(3,1)*T,0,Wtbb(1,1)*T;

Wtbb(3,1)*T,Wtbb(2,1)*T,-Wtbb(1,1)*T,0]

Q=((1-

dltaTita0_fang/8)*eye(4)+0.5*dltaTita)*[q0(k);q1(k);q2(k);q3(k)]; q0(k+1)=Q(1);

q1(k+1)=Q(2);

q2(k+1)=Q(3);

q3(k+1)=Q(4);

end

figure

hold on

i=1:n1;

subplot(1,2,1),plot(i,Vxt(i))%速度误差

title('叠加噪声后的东向速度误差')

subplot(1,2,2),plot(i,Vyt(i))

title('叠加噪声后的的北向速度误差')

figure

hold on

i=1:n1;

subplot(1,2,1),plot(i,L(i)*180/pi)%位置误差

title('叠加噪声后的的纬度误差')

subplot(1,2,2),plot(i,nmda(i)*180/pi)

title('叠加噪声后的的经度误差')

figure

hold on

i=1:n1;

plot(i,Phai(i)*180/pi)%姿态角误差subplot(1,3,1),

title('叠加噪声后的的航向角误差')

figure

hold on

i=1:n1;

plot(i,Thita(i)*180/pi)%subplot(1,3,2),

title('叠加噪声后的俯仰角误差')

figure

hold on

i=1:n1;

plot(i,Gama(i)*180/pi)%subplot(1,3,3),

title('叠加噪声后的横滚角误差')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%轨迹发生器的数据处理%%%%%%%%%%%%%%%

clear,clc

invout=load('E:\惯导实验数据\第四次\实验4.3\第四部分半物理仿真数据生成方法及数据格式说明\IMU数据\invout.dat');

Wx_invout=invout(:,5);

Wy_invout=invout(:,6);

Wz_invout=invout(:,7);

Wibb=[Wx_invout,Wy_invout,Wz_invout];

Fx_invout=invout(:,2);

Fy_invout=invout(:,3);

Fz_invout=invout(:,4);

Fibb=[Fx_invout,Fy_invout,Fz_invout];

L_invout=invout(:,8); %纬度

Jingdu_invout=invout(:,9);%经度

Height_invout=invout(:,10);%高度

N1=size(invout);

n1=N1(1,1);

q0=zeros(n1,1);

q1=zeros(n1,1);

q2=zeros(n1,1);

q3=zeros(n1,1);

Phai=zeros(n1,1);

Thita=zeros(n1,1);

Gama=zeros(n1,1);

Phai(1)=0/180*pi;%偏航初始角

Thita(1)=0*pi/180;%俯仰初始角

Gama(1)=0*pi/180;%横滚初始角

L=zeros(n1,1);

nmda=zeros(n1,1);

Vxt=zeros(n1+1,1);

Vyt=zeros(n1+1,1);

q0(1)=cos(-Phai(1)/2)*cos(Thita(1)/2)*cos(Gama(1)/2)+sin(-

Phai(1)/2)*sin(Thita(1)/2)*sin(Gama(1)/2);

q1(1)=cos(-Phai(1)/2)*sin(Thita(1)/2)*cos(Gama(1)/2)+sin(-Phai(1)/2)*cos(Thita(1)/2)*sin(Gama(1)/2);

q2(1)=cos(-Phai(1)/2)*cos(Thita(1)/2)*sin(Gama(1)/2)-sin(-Phai(1)/2)*sin(Thita(1)/2)*cos(Gama(1)/2);

q3(1)=cos(-Phai(1)/2)*sin(Thita(1)/2)*sin(Gama(1)/2)-sin(-Phai(1)/2)*cos(Thita(1)/2)*cos(Gama(1)/2);

Wie=0.000072921151467;%已经是弧度制

L(1)=40/180*pi;

nmda(1)=116.0/180*pi;

T=0.01;%采样频率为100Hz

Vxt(1)=0;

Vyt(1)=0;

Re=6378245+80;%加高度80米

e=1/298.3;

for k=1:n1

c11=q0(k)^2+q1(k)^2-q2(k)^2-q3(k)^2;

c12=2*(q1(k)*q2(k)+q0(k)*q3(k));

c13=2*(q1(k)*q3(k)-q0(k)*q2(k));

c21=2*(q1(k)*q2(k)-q0(k)*q3(k));

c22=q0(k)^2-q1(k)^2+q2(k)^2-q3(k)^2;

c23=2*(q2(k)*q3(k)+q0(k)*q1(k));

c31=2*(q1(k)*q3(k)+q0(k)*q2(k));

c32=2*(q2(k)*q3(k)-q0(k)*q1(k));

c33=q0(k)^2-q1(k)^2-q2(k)^2+q3(k)^2;

Cnb=[c11,c12,c13

c21,c22,c23

c31,c32,c33];

if abs(c22)>0.0000000000001

Phai(k)=atan(-c21/c22);

end

if abs(c22)>0.0000000000001 & c21>0

Phai(k)=pi/2;

end

if abs(c22)>0.0000000000001 & c21<0

Phai(k)=-pi/2;

end

if abs(c22)>0.0000000000001 & c22>0

Phai(k)=atan(-c21/c22);

end

if abs(c22)>0.0000000000001 & c22>0 & c21>0

Phai(k)=atan(c21/c22)+pi;

end

if abs(c22)>0.0000000000001 & c22>0 & c21<0

Phai(k)=atan(-c21/c22)-pi;

end

Thita(k)=asin(c23);

Gama(k)=-atan(c13/c33);

Cbn=inv(Cnb);

Aibn=Cbn*Fibb(k,:)';

Rxt=Re/(1-e*(sin(L(k))*sin(L(k))));

axt=Aibn(1,1)+2*Wie*sin(L(k))*Vyt(k)+Vyt(k)*Vxt(k)*tan(L(k))/Rxt; ayt=Aibn(2,1)-2*Wie*sin(L(k))*Vxt(k)-Vxt(k)*Vxt(k)*tan(L(k))/Rxt;

Vxt(k+1)=axt*T+Vxt(k);

Vyt(k+1)=ayt*T+Vyt(k);

Ryt=Re/(1+2*e-3*e*(sin(L(k))*sin(L(k))));

L(k+1)=0.5*T*(Vyt(k+1)+Vyt(k))/Ryt+L(k);

nmda(k+1)=0.5*T*(Vxt(k+1)+Vxt(k))/Rxt*sec(L(k))+nmda(k);

Wenn=[-Vyt(k)/Ryt;Vxt(k)/Rxt;Vxt(k)/Rxt*tan(L(k))];%课本86页4.2-38式

Winn=Wenn+[0;Wie*cos(L(k));Wie*sin(L(k))];

Winb=Cnb*Winn;

Wtbb=Wibb(k,:)'-Winb;

dltaTita0_fang=(Wtbb(1,1)*T)^2+(Wtbb(2,1)*T)^2+(Wtbb(3,1)*T)^2;

dltaTita=[0,-Wtbb(1,1)*T,-Wtbb(2,1)*T,-Wtbb(3,1)*T;

Wtbb(1,1)*T,0,Wtbb(3,1)*T,-Wtbb(2,1)*T;

Wtbb(2,1)*T,-Wtbb(3,1)*T,0,Wtbb(1,1)*T;

Wtbb(3,1)*T,Wtbb(2,1)*T,-Wtbb(1,1)*T,0]

Q=((1-

dltaTita0_fang/8)*eye(4)+0.5*dltaTita)*[q0(k);q1(k);q2(k);q3(k)]; q0(k+1)=Q(1);

q1(k+1)=Q(2);

q2(k+1)=Q(3);

q3(k+1)=Q(4);

end

figure

hold on