985工科博士毕业，专攻定位、导航和滤波等算法研究。从业10年，主要使用MATLAB

本文介绍了基于PSINS工具箱的二维平面EKF滤波仿真程序。该程序通过构建包含加速、匀速、转弯等运动特性的轨迹，在添加IMU噪声后，使用15维扩展卡尔曼滤波器融合航向角、速度和位置观测信息。仿真结果显示滤波显著提高了速度和位置估计精度，误差曲线和热力图直观展示了滤波效果。程序还提供了详细的误差统计指标（MAE、RMSE等）和性能评估表格，验证了EKF在二维导航中的有效性。该仿真为平面导航系统设计提供了实用参考，完整MATLAB代码可通过文末链接获取。

PSINS（Precision Strapdown Inertial Navigation System）是面向高精度惯性导航算法开发的MATLAB工具箱，集成了150+个导航相关函数与20+种典型场景案例。惯性器件误差建模（陀螺随机游走、加速度计零偏）组合导航算法（GNSS/INS松耦合/紧耦合）初始对准与标定（静态基座对准、行进间对准）轨迹生成与可视化（三维轨迹动画、误差椭圆绘制）

【代码】PSINS工具箱函数介绍——myfig。

kfplot是图像绘制的函数，用于绘制关于滤波（前/后）的误差图像。本文所述的代码需要基于PSINS工具箱

pvtplot是绘图函数，用于绘制位置、速度、时间信息。

gpssimu是生成GPS的位置和速度信息的函数

trjfile是导入轨迹数据的函数，在文件夹"psins \base\io"里面，用于导入轨迹相关的mat文件。imu即为imu的数据，加速度计3维、陀螺仪也是3维，第七维是时间戳。avp是姿态、速度、位置的数据。avp0是初始时刻的avp。ts是时间间隔（采样周期）

trjsimu是生成轨迹的的函数，用于初始化IMU数据和avp。

imuplot是绘图的函数，用于绘制IMU的加速度计和陀螺仪参数的曲线图。

dxyz2pos函数用于将两点之间的xyz三轴坐标差值换算为纬经高。

在PSINS工具箱里面的distance用于计算轨迹的路程。输入轨迹后，即可由这个函数迭代计算出来路程（也就是每个点之间的距离之和）。函数输入为位置（纬度+精度+高度）和时间戳。输出为计算出的来的路程。kfinit是kf的参数初始化函数，用于初始化滤波参数。左上角的图是路程与时间的关系，表头显示的数字。左下角是高度与时间的关系。

avp的误差设置函数介绍

trj.imu和得到的imu都是n行7列的矩阵，n是采样点的个数，7列分别是陀螺仪的3维数据+加速度计的3维数据+1列时间戳。输入不含误差的IMU（trj.imu）和imuerr，即可输出加上了误差的IMU数据了。是存储了IMU误差信息的结构体，由"

insinit是初始化INS系统的函数

【注】本文所述的函数kfinit0不同于kfinit，后者的讲解链接见：PSINS工具箱函数介绍——kfinitkfinit是kf的参数初始化函数，用于初始化滤波参数本文所述的代码需要基于PSINS工具箱，工具箱的讲解：kf = kfinit0(kf, nts);kf是关于卡尔曼滤波的参数，是一个结构体，参考kfinit这个函数：https://blog.csdn.net/callmeup/article/details/140779776另一个输入值nts是INS的采样周期，一般在kfinit这

介绍工具箱里面r2d这个小函数的作用。

insplot;是速度曲线图，其中蓝色、红色、黄色分别为东向、北向、天向的速度。第一行的两幅图为三个姿态角的曲线图。的紫色曲线为速度大小

kfinit这个函数的字面意思是：kf的初始化。这里面的$"kf"$表示的不是狭义的线性卡尔曼滤波，而是所有卡尔曼滤波类的滤波方法，后面不管是EKF，还是UKF、CKF，都是用这个kfinit来初始化的。

介绍gpsplot函数的原理和作用。

每一行是一个时间序列下计算的结果，其中1~3列为姿态角误差（由avp-trj.avp），4-6列为速度误差，7-9列为位置误差（纬经高），第10列是时间戳。第一个trj.avp是用于对比的基准avp，第二项avp是需要对比的avp。avpcmpplot这个函数的字面意思是：avp的比较和绘图。【注】avp的格式要求：每行表示一个时间1~3列为姿态（如上图，9659行、10列的矩阵。），4-6列为速度（），7-9列为位置（