【PSINS工具箱】基于工具箱的EKF滤波改为速度观测的方法

感谢严老师的无私奉献 test_SINS_GPS_153——15维状态和位置量测的SINS/GPS松组合 glvs——'全局变量,详情请参考glvf函数' psinstypedef(153);——'全局变量,主要设置状态转移矩阵的维数、量测矩阵的维数和画图' trj = trjfile('trj10ms.mat');——'轨迹数据，主要包括：参考imu数据，参考姿态、速度和位置数据，采样间隔。一般由'trjsegment'和'trjsimu'函数生成' % initial settings [n..

代码】PSINS工具箱中轨迹生成工具详细解析。

PSINS（Precise Strapdown Inertial Navigation System 高精度捷联惯导系统算法）工具箱由西北工业大学自动化学院惯性技术教研室严恭敏老师开发和维护。工具箱分为Matlab和C++两部分。主要应用于捷联惯导系统的数据处理和算法验证开发，它包括惯性传感器数据分析惯组标定初始对准惯导AVP（姿态-速度-位置）更新解算、组合导航Kalman滤波等功能。C++部分采用VC6编写，可以用于嵌入式开发。如果你之前还没接触过PSINS工具箱，强烈建议先去看严老师的视频讲解。

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

本文介绍了一种基于PSINS工具箱的二维组合导航仿真方法，采用扩展卡尔曼滤波（EKF）融合捷联惯性导航（SINS）与外部观测（GPS位置+DVL速度）。代码实现了完整的导航流程：轨迹生成、传感器误差模拟、EKF滤波（15状态模型）及性能评估。通过多种可视化图表（误差曲线、轨迹对比、统计特性等）全面展示了滤波效果，位置精度达米级。该方法适用于地面车辆等二维平面导航场景，提供可复现的MATLAB实现方案。

背景与应用本代码旨在通过 CKF 方法优化速度观测，使用 GNSS 数据来提高惯性导航系统的定位精度。CKF 是一种改进的卡尔曼滤波器，适用于处理非线性系统的状态估计问题。输入数据代码从名为的文件中读取轨迹数据，该数据包含了 IMU（惯性测量单元）和 GNSS 的观测信息。初始的 IMU 数据通过imuadderr函数添加了噪声，模拟现实环境中传感器的误差。

背景与应用本代码旨在通过 CKF 方法优化速度观测，使用 GNSS 数据来提高惯性导航系统的定位精度。CKF 是一种改进的卡尔曼滤波器，适用于处理非线性系统的状态估计问题。输入数据代码从名为的文件中读取轨迹数据，该数据包含了 IMU（惯性测量单元）和 GNSS 的观测信息。初始的 IMU 数据通过imuadderr函数添加了噪声，模拟现实环境中传感器的误差。

本程序以位置、速度和航向角为观测量，构造157（15维状态量、7维观测量）的滤波，采用扩展卡尔曼滤波（EKF）进行状态估计。

使用PSINS工具箱和ROS/C++进行调参

imuerrset是设置IMU误差的函数，用于输入IMU的一些误差参数（如：陀螺仪零偏、加速度计零偏、陀螺仪随机游走等值），经此函数处理后，得到"imuerr"这个结构体。

原始 Markdown文档、Visio流程图、XMind思维导图见：https://github.com/LiZhengXiao99/Navigation-Learning。

在有工具箱的情况下，直接运行代码，即可。

载体坐标系b： 右前上 地理坐标系（导航系）：东北天？ 载体姿态角速率、加速度作为输入，积分得到位置、姿态。不考虑侧滑角和攻角。 轨迹包括几个部分，匀速、匀加速、俯仰、横滚、转弯 ...........................

天文导航（cns）+ins组合导航和gps+ins导航的结果对比，MATLAB的源代码，基于psins工具箱。工具箱介绍：PSINS工具箱是一个开源的惯性导航系统（INS）工具箱，用于处理惯性测量单元（IMU）数据进行导航和定位。它是一个使用MATLAB编写的软件工具，提供了一系列功能和算法，可以用于处理IMU的原始数据并估计航姿（航向、俯仰、横滚）、速度和位置等导航参数。

本文给出PSINS工具箱test_SINS_GPS_153的代码注释

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

文章目录习惯约定与常用变量符号PSINS全局变量结构体glv（global variable）坐标系定义姿态阵/姿态四元数/姿态角IMU采样数据AVP导航参数误差参数其他导入数据文件与数据提取转换导入文件数据有以下方式：数据提取转换举例绘图显示绘图辅助函数传感器数据绘图导航结果绘图进度条函数姿态阵/姿态四元数/欧拉角/等效旋转矢量之间转换 习惯约定与常用变量符号 PSINS全局变量结构体glv（global variable） 运行glvs脚本文件，内部实际调用的是glvf函数，这个函数就是可以初始化全局变

机器人运动学几何参数辨识（MATLAB实现）