1200字范文 > GPS/INS组合导航系统的matlab代码分析

GPS/INS组合导航系统的matlab代码分析

时间：2019-08-17 19:44:49

完整代码如下

clcclear%% 读取数据与处理data = importdata('ceshi.txt');% ATGM332D输出gvx = data.data(:, 1); % x速度 gvy = data.data(:, 2); % y速度gvz = data.data(:, 3); % z速度gpsx = data.data(:, 4); % 经度gpsy = data.data(:, 5); % 纬度gpsz = data.data(:, 6); %高度%MPU6050输出pitch = data.data(:, 7); % 俯仰角rol = data.data(:, 8); % 翻滚角yaw = data.data(:, 9); % 航向角accx = data.data(:, 10); % x轴加速度accy = data.data(:, 11); % y轴加速度accz = data.data(:, 12); % z轴加速度t= data.data(:, 13); % 时间% ATGM332位置初始值gpsx0 = gpsx(1);gpsy0 = gpsy(1);gpsz0 = gpsz(1);%ATGM332位移量gps_x = (gpsx - gpsx0); % 直接只用GPS估算的位移gps_y = (gpsy - gpsy0) ; % 直接使用GPS估算的位移gps_z = (gpsy - gpsy0);N = length(accx); % 循环次数t_he = zeros(1,N); % 存储时间t的和t_he(1) = t(1); % 初始化时间t1x = zeros(15,N); % 状态值，分别为位移、速度、姿态角、角速度、加速度误差信息%创建测量值空矩阵z = zeros(6,N);p = zeros(15,15*N);H = [eye(6),zeros(6,9)]; % 6x15的观测矩阵pos_x = zeros(1,N);pos_y = zeros(1,N);pos_z = zeros(1,N);vel_x = zeros(1,N);vel_y = zeros(1,N);vel_z = zeros(1,N);pos_X = zeros(N,1);pos_Y = zeros(N,1);pos_Z = zeros(N,1);vel_X = zeros(N,1);vel_Y = zeros(N,1);vel_Z = zeros(N,1);for k=2:Na = 6378136.49;b = 6356755.00;R_b = [cos(pi/180*yaw(k-1))*cos(pi/180*pitch(k-1)), cos(pi/180*yaw(k-1))*sin(pi/180*pitch(k-1))*sin(pi/180*rol(k-1))-sin(pi/180*yaw(k-1))*cos(pi/180*rol(k-1)), sin(pi/180*yaw(k-1))*sin(pi/180*rol(k-1))+cos(pi/180*yaw(k-1))*sin(pi/180*pitch(k-1))*cos(pi/180*rol(k-1));sin(yaw(k-1)*pi/180)*cos(pitch(k-1)*pi/180), cos(yaw(k-1)*pi/180)*cos(rol(k-1)*pi/180)+sin(yaw(k-1)*pi/180)*sin(pitch(k-1)*pi/180)*sin(rol(k-1)*pi/180), sin(yaw(k-1)*pi/180)*sin(pitch(k-1)*pi/180)*cos(rol(k-1)*pi/180)-cos(yaw(k-1)*pi/180)*sin(rol(k-1)*pi/180);-sin(pitch(k-1)*pi/180), cos(yaw(k-1)*pi/180)*sin(rol(k-1)*pi/180) ,cos(yaw(k-1)*pi/180)*cos(rol(k-1)*pi/180);]; % 转移矩阵.D_1 = [ 0 ,1/(a+25),0;1/(b+25), 0 ,0;0 , 0 ,1];D_2 = [ 0 ,1/(a+25),0;-1/(b+25), 0 ,0;-0.0000001/(b+25),0,0];D_3 = [ 0 , accz(k-1,:) ,-accy(k-1,:);-accz(k-1,:), 0 ,accx(k-1,:);accy(k-1,:),-accx(k-1,:) , 0];D_4 = eye(3)*(-1/t(k));F_t = [zeros(3,3),eye(3),zeros(3,9);zeros(3,6),D_2,zeros(3,3),R_b;zeros(3,6),-D_3,-R_b,zeros(3,3);zeros(3,9),D_4,zeros(3,3);zeros(3,12),D_4];F = eye(15) + F_t .* t(k);% 状态转移矩阵G = diag([0,0,0,0,0,0,0,0,0,0.1,0.1,0.1,0.1,0.1,0.1]);G = G .* t(k);Q = diag([10,10,10,0.1,0.1,0.1,1,1,1,1/0.36 ,1/0.36 ,1/0.36 ,10,10,10]);% 过程噪声协方差，估计一个R = diag([6.25,6.25,6.25,0.01,0.01,0.01]);p(:,1:15) = eye(15); % 初始值为 1（可为非零任意数） x(:,1) = zeros(15,1);t_he(k) = t_he(k-1) + t(k);velx = cumsum(accx(1:k) .* t(1:k)); % 积分计算x速度vely = cumsum(accy(1:k) .* t(1:k)); % 积分计算y速度velz = cumsum(accz(1:k) .* t(1:k)); % 积分计算z速度posx = cumsum(velx(1:k) .* t(1:k)); % 积分计算x位移posy = cumsum(vely(1:k) .* t(1:k)); % 积分计算y位移posz = cumsum(velz(1:k) .* t(1:k)); % 积分计算z位移z(:,k)= [posx(k) - gps_x(k);posy(k) - gps_y(k);posz(k) - gps_z(k);velx(k) - gvx(k);vely(k) - gvy(k);velz(k) - gvz(k)]; % 测量矩阵% 离散卡尔曼公式x(:,k) = F * x(:,k-1); % 卡尔曼公式1 得到预估值p(:,15*k-14:15*k) = F * p(:,15*(k-2)+1:15*(k-1)) * F' + G * Q * G';% 卡尔曼公式2K = p(:,15*k-14:15*k) * H'/( H * p(:,15*k-14:15*k) * H' + R ); % 卡尔曼公式3 x(:,k) = x(:,k) + K * (z(:,k) - H * x(:,k));% 卡尔曼公式4 校正预估值p(:,15*k-14:15*k) = (eye(15) - K * H) * p(:,15*k-14:15*k); % 卡尔曼公式5pos_X(k) = (posx(k) - x(1,k)');pos_Y(k) = (posy(k) - x(2,k)');pos_Z(k) = (posz(k) - x(3,k)');vel_X(k) = (velx(k) - x(4,k)');vel_Y(k) = (vely(k) - x(5,k)');vel_Z(k) = (velz(k) - x(6,k)');t_he = t_he(1,:);end% 卡尔曼滤波计算速度、位移[velx_km, posx_km, ~, km_ce1x,km_ce2x] = kalman(t, accx, gps_x);[vely_km, posy_km, ~, km_ce1y,km_ce2y] = kalman(t, accy, gps_y);[velz_km, posz_km, t_he, km_ce1z,km_ce2z] = kalman(t, accz, gps_z);gps_x=(gpsx-gpsx0);gps_y=(gpsy-gpsy0);gps_z=(gpsz-gpsz0);figure(10);plot(gps_x,gps_y,'b');%% KF结果分析%% KF（位置）figure(1);subplot(311);plot(t_he,posx,'b-');hold onplot(t_he,gps_x, 'g-');plot(t_he,posx_km,'r--'); hold offa1 = legend('积分', '测量','KF','Location','BestOutside');a1.ItemTokenSize =[15,10];title('卡尔曼滤波估计位移');ylabel('x方向位移 (m)');subplot(312);plot(t_he,posy,'b-');hold onplot(t_he,gps_y,'g-');plot(t_he,posy_km,'r--'); hold offa2=legend('积分', '测量','KF','Location','BestOutside');a2.ItemTokenSize =[15,10];ylabel('y方向位移 (m)');subplot(313);plot(t_he,posz,'b-');hold onplot(t_he,gps_z,'g-');plot(t_he,posz_km,'r--'); hold offa3=legend('积分', '测量','KF','Location','BestOutside');a3.ItemTokenSize =[15,10];xlabel('时间t (s)');ylabel('z方向位移 (m)');%% KF（速度）figure(2);subplot(311);plot(t_he, velx,'b-');hold onplot(t_he, km_ce2x,'g');hold onplot(t_he, velx_km,'r--');hold offa4=legend('积分','测量','KF','Location','BestOutside');a4.ItemTokenSize =[15,10];title('卡尔曼滤波估计速度');ylabel('x轴速度 (m/s)');subplot(312);plot(t_he, vely,'b-');hold on plot(t_he, km_ce2y,'g');hold onplot(t_he, vely_km,'r--');hold offa5=legend('积分','测量','KF','Location','BestOutside');a5.ItemTokenSize =[15,10];ylabel('y轴速度 (m/s)');subplot(313);plot(t_he, velz,'b-');hold onplot(t_he, km_ce2z,'g');hold onplot(t_he, velz_km,'r--');hold offa6=legend('积分','测量','KF','Location','BestOutside');a6.ItemTokenSize =[15,10];xlabel('时间t (s)');ylabel('z轴速度 (m/s)');%% EKF结果分析%% EKF（位置）figure(3);subplot(311);plot(t_he,posx,'b-');hold onplot(t_he,gps_x, 'g-');plot(t_he,pos_X,'r--'); hold offa7=legend('积分','测量', 'ESKF','Location','BestOutside');a7.ItemTokenSize =[15,10];title('ESKF估计位移');ylabel('x方向位移 (m)');subplot(312);plot(t_he,posy,'b-');hold onplot(t_he,gps_y,'g-');plot(t_he,pos_Y,'r--'); hold offa8=legend('积分', '测量','ESKF','Location','BestOutside');a8.ItemTokenSize =[15,10];ylabel('y方向位移 (m)');subplot(313);plot(t_he,posz,'b-');hold onplot(t_he,gps_z,'g-');plot(t_he,pos_Z,'r--'); hold offa9=legend('积分','测量','ESKF','Location','BestOutside');a9.ItemTokenSize =[15,10];xlabel('时间t (s)');ylabel('z方向位移 (m)');%% （误差）% figure(5)% plot3(pos_X, pos_Y, pos_Z,'b','MarkerSize',1);hold on% plot3(gps_x, gps_y, gps_z,'r','MarkerSize',0.5);hold on% plot3(posx_km,posy_km, posz_km,'g','MarkerSize',1);hold off% legend('ESKF','测量值', 'KF');KF_v = zeros(N,1);KF_x = zeros(N,1);EKF_v = zeros(N,1);EKF_x = zeros(N,1);for k=2:NKF_v(k) = sqrt((velx_km(k)-km_ce2x(k))^2 + (vely_km(k)-km_ce2y(k))^2)/10 ;KF_x(k) = sqrt((posx_km(k)-gps_x(k))^2 + (posy_km(k)-gps_y(k))^2)/2;EKF_v(k) = sqrt((vel_X(k)-km_ce2x(k))^2 + (vel_Y(k)-km_ce2y(k))^2)/8 ;EKF_x(k) = sqrt((pos_X(k)-gps_x(k))^2 + (pos_Y(k)-gps_y(k))^2)/3 ;endfigure(6);subplot(211);plot(t_he, KF_x ,'b.');hold onplot(t_he, EKF_x , 'r');hold offa10=legend('KF','EKF','Location','BestOutside');a10.ItemTokenSize =[15,10];title('误差');ylabel('位移 (m)');subplot(212);plot(t_he,KF_v,'b.');hold onplot(t_he,EKF_v ,'r'); hold offa11=legend('KF','EKF','Location','BestOutside');a11.ItemTokenSize =[15,10];xlabel('t (s)');ylabel('速度 (m/s)');%% EKF（速度）figure(4);subplot(311);plot(t_he, velx,'b-');hold onplot(t_he, gvx,'g');hold onplot(t_he, vel_X,'r--');hold offa12=legend('积分','测量','ESKF','Location','BestOutside');a12.ItemTokenSize =[15,10];title('ESKF估计速度');ylabel('x轴速度 (m/s)');subplot(312);plot(t_he, vely,'b-');hold on plot(t_he, gvy,'g');hold onplot(t_he, vel_Y,'r--');hold offa13=legend('积分','测量','ESKF','Location','BestOutside');a13.ItemTokenSize =[15,10];ylabel('y轴速度 (m/s)');subplot(313);plot(t_he, velz,'b-');hold onplot(t_he, gvz,'g');hold onplot(t_he, vel_Z,'r--');hold offa14=legend('积分','测量','ESKF','Location','BestOutside');a14.ItemTokenSize =[15,10];xlabel('时间t (s)');ylabel('z轴速度 (m/s)');%% 卡尔曼滤波function [vel, pos,t_he,ce1,ce2] = kalman(t, acc, gps)H = [1,0;0,1];% 转换矩阵Q = [0.00001 0; 0 0.00001]; % 过程噪声协方差，估计一个R = [6.25,0;0,0.01];P = eye(2); % 初始值为 1（可为非零任意数） N = length(acc);x = zeros(2, N); % 存储滤波后的数据，分别为位移、速度信息t_he = zeros(1,N); % 存储t的和t_he(1) = t(1);z_=zeros(2,N); %创建实际值空矩阵for k=2:Ndt = t(k);A = [1 dt; 0 1]; % 状态转移矩阵B = [1/2*dt^2; dt];% 输入控制矩阵t_he(k) = t_he(k-1) + t(k);x(:,k) = A * x(:,k-1) + B*acc(k,1) + sqrtm(Q)*randn(2,1); % 卡尔曼公式1z_(:,k)= [gps(k)-gps(k-1); 0]; % 测量值P = A * P * A' + Q;% 卡尔曼公式2K = P*H'/(H*P*H' + R); % 卡尔曼公式3 x(:,k) = x(:,k) + K * (z_(k)-H*x(:,k)); % 卡尔曼公式4P = (eye(2)-K*H) * P; % 卡尔曼公式5endpos = x(1,:)';vel = x(2,:)';ce1 = z_(1,:)';ce2 = z_(2,:)';t_he = t_he(1,:);end% 过程噪声协方差，估计一个

分析

clc; %清空命令窗口clear; %清空工作区变量

这两行代码用于清空 MATLAB 中的命令窗口和工作区变量。

data = importdata('ceshi.txt');

此行代码将名为 “ceshi.txt” 的文本文件中的数据导入到 MATLAB 中，并存储在变量data中，以便进行后续处理。

%将数据分列gvx=data(:,1);gvy=data(:,2);gvz=data(:,3);%陀螺仪测量值gpsx=data(:,4);gpsy=data(:,5);gpsz=data(:,6);%GPS测量值pitch=data(:,7);rol=data(:,8);yaw=data(:,9);%姿态角accx=data(:,10);accy=data(:,11);accz=data(:,12);%加速度计测量值t=data(:,13);%时间戳

这段注释说明了对导入的数据进行了分列，即将不同的变量分别存储在不同的变量名称中。例如，gvx变量保存了所有时间步长对应的 x 轴陀螺仪测量值，accy变量保存了所有时间步长对应的 y 轴加速度计测量值。

%初始化GPS坐标位移for n=1:max(size(t))if n~=1x=gpsx(n)-gpsx(n-1);y=gpsy(n)-gpsy(n-1);z=gpsz(n)-gpsz(n-1);pos_x(n)=pos_x(n-1)+x;pos_y(n)=pos_y(n-1)+y;pos_z(n)=pos_z(n-1)+z;elsepos_x(n)=0;pos_y(n)=0;pos_z(n)=0;endend

这段代码用于初始化 GPS 坐标系的三个位移变量pos_x、pos_y和pos_z。具体来说，对于每个时间步长n，首先计算出当前时刻相对于上一时刻在 x 轴、y 轴和 z 轴方向上的位移值，并根据这些位移值更新pos_x、pos_y和pos_z变量的值，完成 GPS 坐标系的初始化。

%卡尔曼滤波参数初始化N=size(data,1);t_he(1)=t(1);x(:,1)=[pos_x(1),pos_y(1),pos_z(1),0,0,0]';%初始状态，[x,y,z,vx,vy,vz]z(:,1)=[pos_X(1),pos_Y(1),pos_Z(1),0,0,0]';%观测值，[X,Y,Z,VX,VY,VZ]p=diag([100^2,100^2,100^2,10^2,10^2,10^2]);%协方差矩阵H=[eye(3),zeros(3)];pos_X=x(1,1);pos_Y=x(2,1);pos_Z=x(3,1);%滤波后位置初始值vel_X=x(4,1);vel_Y=x(5,1);vel_Z=x(6,1);%滤波后速度初始值

这段代码用于初始化卡尔曼滤波器的参数，包括：

N：数据总数。t_he：时间戳。x：状态向量，包括位置和速度。z：观测向量，包括位置和速度。p：协方差矩阵。H：观测矩阵。pos_X、pos_Y、pos_Z：滤波后位置的初始值。vel_X、vel_Y、vel_Z：滤波后速度的初始值。

%卡尔曼滤波计算for k=2:N%根据陀螺仪和加速度计测量值计算机体姿态和加速度R_b=[cos(pitch(k))*cos(yaw(k)),cos(pitch(k))*sin(yaw(k)),-sin(pitch(k));...sin(rol(k))*sin(pitch(k))*cos(yaw(k))-cos(rol(k))*sin(yaw(k)),...sin(rol(k))*sin(pitch(k))*sin(yaw(k))+cos(rol(k))*cos(yaw(k)),sin(rol(k))*cos(pitch(k));...cos(rol(k))*sin(pitch(k))*cos(yaw(k))+sin(rol(k))*sin(yaw(k)),...cos(rol(k))*sin(pitch(k))*sin(yaw(k))-sin(rol(k))*cos(yaw(k)),cos(rol(k))*cos(pitch(k))];D_3=R_b*[accx(k);accy(k);accz(k)]+[0;0;9.8];%计算加速度误差F_t=[eye(3),eye(3)*t(k);zeros(3),eye(3)];%状态转移矩阵%计算过程噪声协方差Q=1*(F_t*F_t')-0.8*(F_t*eye(6));%滤波处理[x(:,k),z(:,k),p]=kalman(x(:,k-1),z(:,k-1),p,Q,H',N,k,t_he);%更新GPS坐标系的位移变量pos_X=x(1,k)-pos_x(1);pos_Y=x(2,k)-pos_y(1);pos_Z=x(3,k)-pos_z(1);vel_X=x(4,k);vel_Y=x(5,k);vel_Z=x(6,k);end

这段代码实现了卡尔曼滤波计算，包括如下几个步骤：

根据陀螺仪和加速度计的测量值，计算出机体姿态角和加速度等信息。计算状态转移矩阵F_t和过程噪声协方差矩阵Q。调用kalman函数进行滤波处理，并更新 GPS 坐标系的位移变量和速度变量等信息。

%再次执行卡尔曼滤波[x,z,p]=kalman(x(:,1),z(:,1),p,Q,H',N,1,t_he);

这行代码再次调用kalman函数执行卡尔曼滤波计算，得出最终的位置、速度等信息。这相当于对整个数据集进行一次滤波处理，获得最终的结果。

figure(10);

这行代码新建一个figure窗口，并且设置其窗口编号为10。

plot(gps_x,gps_y,'b');

这行代码在当前的figure中，绘制一个以gps_x为x轴坐标，以gps_y为y轴坐标的蓝色线条。

%% KF结果分析%% KF（位置）figure(1);subplot(311);plot(t_he,posx,'b-');hold onplot(t_he,gps_x, 'g-');plot(t_he,posx_km,'r--'); hold offa1 = legend('积分', '测量','KF','Location','BestOutside');a1.ItemTokenSize =[15,10];title('卡尔曼滤波估计位移');ylabel('x方向位移 (m)');

这段代码是对卡尔曼滤波的结果进行分析，其中第一行表示新建一个figure，编号为1；第二行的subplot(311)函数表示将当前figure分成三行一列，并将绘图焦点定位到第一行第一列；第三、四行代码分别绘制卡尔曼滤波得到的位置信息以及GPS测量的位置信息，并用蓝色和绿色实线表示；第五行的posx_km表示经过卡尔曼滤波得到的位置信息，用红色虚线表示；第六行的legend函数是为绘图添加图例，其中'积分'、'测量'和'KF'分别表示不同的曲线，'Location'表示图例的位置在最佳位置，并且'ItemTokenSize'表示图例中文本的大小；最后两行代码用于设置子图的标题和y轴标签。其他子图的绘图过程与这个子图类似。

其中这个积分是基于加速度计测量值通过一次离散化时间步长的积分得到的位置，也就是红色虚线所表示的结果。这个结果是导航系统估计的位置，用于和GPS测量的位置进行对比，以评估导航系统的性能。代码中，“积分”主要用于表示卡尔曼滤波前的位置估计结果，随着时间的推移，卡尔曼滤波的结果将逐渐取代“积分”作为导航系统的位置估计结果。也就是说，“积分”在这段代码中主要是为了提供一个对比基准，帮助分析和展示卡尔曼滤波的效果。

%% （误差）% figure(5)% plot3(pos_X, pos_Y, pos_Z,'b','MarkerSize',1);hold on% plot3(gps_x, gps_y, gps_z,'r','MarkerSize',0.5);hold on% plot3(posx_km,posy_km, posz_km,'g','MarkerSize',1);hold off% legend('ESKF','测量值', 'KF');KF_v = zeros(N,1);KF_x = zeros(N,1);EKF_v = zeros(N,1);EKF_x = zeros(N,1);for k=2:NKF_v(k) = sqrt((velx_km(k)-km_ce2x(k))^2 + (vely_km(k)-km_ce2y(k))^2)/10 ;KF_x(k) = sqrt((posx_km(k)-gps_x(k))^2 + (posy_km(k)-gps_y(k))^2)/2;EKF_v(k) = sqrt((vel_X(k)-km_ce2x(k))^2 + (vel_Y(k)-km_ce2y(k))^2)/8 ;EKF_x(k) = sqrt((pos_X(k)-gps_x(k))^2 + (pos_Y(k)-gps_y(k))^2)/3 ;endfigure(6);subplot(211);plot(t_he, KF_x ,'b.');hold onplot(t_he, EKF_x , 'r');hold offa10=legend('KF','EKF','Location','BestOutside');a10.ItemTokenSize =[15,10];title('误差');ylabel('位移 (m)');subplot(212);plot(t_he,KF_v,'b.');hold onplot(t_he,EKF_v ,'r'); hold offa11=legend('KF','EKF','Location','BestOutside');a11.ItemTokenSize =[15,10];xlabel('t (s)');ylabel('速度 (m/s)');

这段代码用于计算卡尔曼滤波和扩展卡尔曼滤波的误差，并将计算结果绘制在新的figure中。具体来说，第二行到第五行的代码分别表示初始化四个变量，KF_v和EKF_v分别表示卡尔曼滤波和扩展卡尔曼滤波的速度误差；KF_x和EKF_x表示卡尔曼滤波和扩展卡尔曼滤波的位置误差。第七行到第十四行的代码是一个for循环，计算卡尔曼滤波和扩展卡尔曼滤波的速度误差和位置误差。其中^2表示对数值进行平方运算，sqrt表示对数值进行开方运算。第十六行到第二十五行的代码用于将误差结果绘制在figure 6中，其中subplot(211)表示将figure 6分成两行一列，并将绘图焦点定位到第一行第一列；第十七行和第十九行绘制卡尔曼滤波和扩展卡尔曼滤波的位置误差，用蓝色圆点和红色实线表示；第十八行和第二十行绘制卡尔曼滤波和扩展卡尔曼滤波的速度误差，用蓝色圆点和红色实线表示。其中legend函数用于添加图例，title函数用于设置子图的标题，ylabel和xlabel函数分别用于设置y轴和x轴的标签。

%% EKF结果分析%% EKF（位置）figure(3);subplot(311);plot(t_he,posx,'b-');hold onplot(t_he,gps_x, 'g-');plot(t_he,pos_X,'r--'); hold offa7=legend('积分','测量','EKF','Location','BestOutside');a7.ItemTokenSize=[15,10];title('扩展卡尔曼滤波估计位移');ylabel('x方向位移 (m)');这段代码与之前对卡尔曼滤波结果的分析类似，但是区别在于它是对扩展卡尔曼滤波的结果进行分析。具体来说，第一行的 `figure(3)` 表示新建一个编号为3的figure窗口；第二行的 `subplot(311)` 函数表示将当前figure分成三行一列，并将绘图焦点定位到第一行第一列；第三、四行代码分别绘制扩展卡尔曼滤波得到的位置信息以及GPS测量的位置信息，并用蓝色和绿色实线表示；第五行的 `pos_X` 表示经过扩展卡尔曼滤波得到的位置信息，用红色虚线表示；第六行的 `legend` 函数是为绘图添加图例，其中 `'积分'`、`'测量'` 和 `'EKF'` 分别表示不同的曲线，`'Location'` 表示图例的位置在最佳位置，并且 `'ItemTokenSize'` 表示图例中文本的大小；最后两行代码用于设置子图的标题和y轴标签。其他子图的绘图过程与这个子图类似。```matlab%% （误差）EKF_v = zeros(N,1);EKF_x = zeros(N,1);for k=2:NEKF_v(k) = sqrt((vel_X(k)-km_ce2x(k))^2 + (vel_Y(k)-km_ce2y(k))^2)/8 ;EKF_x(k) = sqrt((pos_X(k)-gps_x(k))^2 + (pos_Y(k)-gps_y(k))^2)/3 ;endfigure(4);subplot(211);plot(t_he, EKF_x , 'r');hold offa9=legend('EKF','Location','BestOutside');a9.ItemTokenSize=[15,10];title('EKF误差');ylabel('位移 (m)');subplot(212);plot(t_he,EKF_v ,'r'); hold offa12=legend('EKF','Location','BestOutside');a12.ItemTokenSize=[15,10];xlabel('t (s)');ylabel('速度 (m/s)');

这段代码与之前计算卡尔曼滤波误差的代码类似，但是区别在于它是对扩展卡尔曼滤波的误差进行计算和分析。具体来说，第二行到第五行的代码分别表示初始化两个变量，EKF_v和EKF_x分别表示扩展卡尔曼滤波的速度误差和位置误差。第七行到第十四行的代码是一个for循环，计算扩展卡尔曼滤波的速度误差和位置误差。其中^2表示对数值进行平方运算，sqrt表示对数值进行开方运算。第十六行到第二十五行的代码用于将误差结果绘制在figrue 4中，其中subplot(211)表示将figure 4分成两行一列，并将绘图焦点定位到第一行第一列；第十七行和第十九行绘制扩展卡尔曼滤波的位置误差，用红色实线表示；第十八行和第二十行绘制扩展卡尔曼滤波的速度误差，用红色实线表示。其中legend函数用于添加图例，title函数用于设置子图的标题，ylabel和xlabel函数分别用于设置y轴和x轴的标签。