完整代码如下
代码语言：javascript复制clc
clear
%% 读取数据与处理
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); % 初始化时间t1
x = 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:N
a = 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'; % 卡尔曼公式2
K = 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); % 卡尔曼公式5
pos_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 on
plot(t_he,gps_x, 'g-');
plot(t_he,posx_km,'r--'); hold off
a1 = legend('积分', '测量','KF','Location','BestOutside');
a1.ItemTokenSize =[15,10];
title('卡尔曼滤波估计位移');ylabel('x方向位移 (m)');
subplot(312);
plot(t_he,posy,'b-');hold on
plot(t_he,gps_y,'g-');
plot(t_he,posy_km,'r--'); hold off
a2=legend('积分', '测量','KF','Location','BestOutside');
a2.ItemTokenSize =[15,10];
ylabel('y方向位移 (m)');
subplot(313);
plot(t_he,posz,'b-');hold on
plot(t_he,gps_z,'g-');
plot(t_he,posz_km,'r--'); hold off
a3=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 on
plot(t_he, km_ce2x,'g');hold on
plot(t_he, velx_km,'r--');
hold off
a4=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 on
plot(t_he, vely_km,'r--');hold off
a5=legend('积分','测量','KF','Location','BestOutside');
a5.ItemTokenSize =[15,10];
ylabel('y轴速度 (m/s)');
subplot(313);
plot(t_he, velz,'b-');hold on
plot(t_he, km_ce2z,'g');hold on
plot(t_he, velz_km,'r--');hold off
a6=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 on
plot(t_he,gps_x, 'g-');
plot(t_he,pos_X,'r--'); hold off
a7=legend('积分','测量', 'ESKF','Location','BestOutside');
a7.ItemTokenSize =[15,10];
title('ESKF估计位移');ylabel('x方向位移 (m)');
subplot(312);
plot(t_he,posy,'b-');hold on
plot(t_he,gps_y,'g-');
plot(t_he,pos_Y,'r--'); hold off
a8=legend('积分', '测量','ESKF','Location','BestOutside');
a8.ItemTokenSize =[15,10];
ylabel('y方向位移 (m)');
subplot(313);
plot(t_he,posz,'b-');hold on
plot(t_he,gps_z,'g-');
plot(t_he,pos_Z,'r--'); hold off
a9=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:N
KF_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 ;
end
figure(6);subplot(211);
plot(t_he, KF_x ,'b.');hold on
plot(t_he, EKF_x , 'r');hold off
a10=legend('KF','EKF','Location','BestOutside');
a10.ItemTokenSize =[15,10];
title('误差');ylabel('位移 (m)');
subplot(212);
plot(t_he,KF_v,'b.');hold on
plot(t_he,EKF_v ,'r'); hold off
a11=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 on
plot(t_he, gvx,'g');hold on
plot(t_he, vel_X,'r--');
hold off
a12=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 on
plot(t_he, vel_Y,'r--');hold off
a13=legend('积分','测量','ESKF','Location','BestOutside');
a13.ItemTokenSize =[15,10];
ylabel('y轴速度 (m/s)');
subplot(313);
plot(t_he, velz,'b-');hold on
plot(t_he, gvz,'g');hold on
plot(t_he, vel_Z,'r--');hold off
a14=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:N
dt = 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); % 卡尔曼公式1
z_(:,k)= [gps(k)-gps(k-1); 0]; % 测量值
P = A * P * A' Q; % 卡尔曼公式2
K = P*H'/(H*P*H' R); % 卡尔曼公式3
x(:,k) = x(:,k) K * (z_(k)-H*x(:,k)); % 卡尔曼公式4
P = (eye(2)-K*H) * P; % 卡尔曼公式5
end
pos = x(1,:)';
vel = x(2,:)';
ce1 = z_(1,:)';
ce2 = z_(2,:)';
t_he = t_he(1,:);
end % 过程噪声协方差，估计一个分析
代码语言：javascript复制clc; %清空命令窗口
clear; %清空工作区变量这两行代码用于清空 MATLAB 中的命令窗口和工作区变量。
代码语言：javascript复制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~=1
x=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;
else
pos_x(n)=0;
pos_y(n)=0;
pos_z(n)=0;
end
end这段代码用于初始化 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。
代码语言：javascript复制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 on
plot(t_he,gps_x, 'g-');
plot(t_he,posx_km,'r--'); hold off
a1 = 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:N
KF_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 ;
end
figure(6);subplot(211);
plot(t_he, KF_x ,'b.');hold on
plot(t_he, EKF_x , 'r');hold off
a10=legend('KF','EKF','Location','BestOutside');
a10.ItemTokenSize =[15,10];
title('误差');ylabel('位移 (m)');
subplot(212);
plot(t_he,KF_v,'b.');hold on
plot(t_he,EKF_v ,'r'); hold off
a11=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 on
plot(t_he,gps_x, 'g-');
plot(t_he,pos_X,'r--'); hold off
a7=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:N
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 ;
end
figure(4);subplot(211);
plot(t_he, EKF_x , 'r');hold off
a9=legend('EKF','Location','BestOutside');
a9.ItemTokenSize=[15,10];
title('EKF误差');ylabel('位移 (m)');
subplot(212);
plot(t_he,EKF_v ,'r'); hold off
a12=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轴的标签。
总结
这段代码主要分析了一个基于GPS信号的导航系统的性能，并通过绘图的方式展示了卡尔曼滤波和扩展卡尔曼滤波的结果和误差。
备注
写的比较着急 如果有分析的不对的地方欢迎指出
