MATLAB卡尔曼的博客

本代码通过扩展观测维度和改进 CKF 算法，实现了高精度的 SINS/GPS 组合导航，适用于无人机、自动驾驶等场景。实验结果表明，滤波后导航误差显著降低，验证了方法的有效性。代码结构清晰，涵盖数据加载、滤波处理、结果分析全流程，可作为惯性导航算法的参考实现。如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者。

基于PSINS工具箱实现了15维状态的无迹卡尔曼滤波（UKF），用于SINS/GPS组合导航系统，观测量为位置、速度和航向角。代码从仅速度观测的UKF153改进而来，扩展了观测维度以提高导航精度。需要先安装PSINS工具箱才能运行，工具箱开源，网络上有下载链接代码段功能描述初始化部分加载参数、设置IMU误差模型、定义轨迹数据、初始化导航系统和滤波器参数。滤波主循环逐帧处理IMU数据，更新惯性导航状态，融合观测数据（航向、速度、位置）进行UKF滤波。结果可视化。

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

该 MATLAB 代码实现了扩展卡尔曼滤波（EKF）、无迹卡尔曼滤波（UKF）和无迹卡尔曼滤波的变体（CKF）的对比，主要用于导航与定位领域，通过处理惯性测量单元（IMU）和GPS数据来估计运动轨迹。

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

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

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

两个滤波函数在组合导航上面应用的对比。UKF是：无迹卡尔滤波，SSUKF是：简化超球面无迹卡尔曼滤波。

累积分布函数（Cumulative Distribution Function，简称CDF）是一种描述随机变量取值的概率分布的函数。CNS探测角度（以恒星为观测）和高度（以地球为观测），滤波时观测量为角度（三维）+高度（一维）。其中e是自然对数的底，π是圆周率。这个累积分布函数可以用来计算X小于等于某个具体取值x的概率。累积分布函数可以用来计算和描述随机变量取值的概率。使用184的滤波结构：18维状态量、4为观测量。使用183的滤波，观测量为GPS的三维位置。

天文导航系统（celestial navigation system, CNS）是一种用于定位、导航和控制天文观测设备的技术。它通过使用天体的位置和运动信息来帮助天文学家准确地定位天体，并进行观测和研究。天文导航系统的主要组成部分包括天文目标数据库、定位系统和导航控制系统。天文目标数据库储存了大量天体的位置、亮度、距离等信息，并以数字化的形式提供给导航系统使用。定位系统是通过使用天体的天文数据和测量设备，例如望远镜、摄像机等，来确定观测设备的位置和朝向。

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

需要PSINS工具箱才能正常运行。集成了EKF（扩展卡尔忙滤波）和CKF（容积卡尔曼滤波），包含了滤波结果的对比。带中文注释。

PSINS中，trj.wat的形式

PSINS工具箱里面的trj.avp、trj.avp0等中的avp指的是姿态（attitude）、速度（velocity）和位置（position），最后加一个时间戳t：avp=[att;vn;pos;t]。

西工大的PSINS工具箱是一种用于惯性导航系统的开源软件工具包。它提供了一系列算法和功能，可用于处理惯性测量单元（IMU）和全球定位系统（GPS）等传感器数据，以实现精确的导航和定位。西工大的PSINS工具箱在导航中的作用是实现精确的姿态解算、导航解算和集成导航，并提供参数标定功能，以提高导航系统的精度和稳定性。，工具箱没有进行相关的研究，这里基于工具箱，以主从式协同导航为例，来尝试设计一个基于相对距离观测的协同导航。

PSINS工具箱是一种用于惯性导航系统的开发和测试的工具集。PSINS是"Position and Attitude Integrated Navigation System"的缩写，指的是一种将位置和姿态信息集成在一起的导航系统。PSINS工具箱提供了一系列的功能模块，包括姿态解算、位置解算、速度解算、姿态更新、导航滤波等。使用PSINS工具箱可以方便地实现惯性导航系统的开发和测试，并可以在不同的硬件平台上进行移植和应用。

借助工具箱完成底层的复杂计算是很有必要的。另一方面，也能利用工具箱进行惯导解算方面的学习。

LaTeX的入门笔记

本函数需要基于PSINS工具箱。输出误差统计特性的MATLAB函数

工具箱本身没有xyz的距离差转纬经高的差值的函数，根据工具箱现有函数名命名规则，编写一个demo，可能不是很严谨，但目前来说够用了。

PSINS的kfinit没有对应的15为状态、1维观测的滤波，但是实际上仅高度可观测的情况在实际上是需要的（无GNSS、只有气压高度计），这里给出出去改后的kf初始化函数。

输出的seg里面的wat是一个8列的矩阵，第一列代表每一个阶段的持续时间，第二列代表该阶段的速度（大小），3。• Wat：轨迹片段，与trjsegment函数一起使用，这里即为输出的seg的seg.wat。输出值seg是轨迹片段片段，作者是一段一段生成轨迹的，每一段的轨迹放在前一段的后面。欧拉角、姿态矩阵、等效旋转矩阵、姿态四元数、运载火箭使用的欧拉角之间的转换。repeats：重复次数，忽略时即视为0，用nargin判断输入参数的量。8为三轴加速度（6为转弯、7为前项，8为爬坡时的）

15维包括：失准角（3）+速度误差（3）+位置误差（3）+陀螺漂移（3）+加速度计常值零漂（3）如图，三个不同维数的SINS/GPS的融合，153表示状态量15维、观测量3维，以此类推。18维包括：15维+惯导与卫星之间的距离误差（3）19维包括：18维+惯导与卫星之间的时间误差（1）增加机动能提高卡尔曼滤波的对准速度。

全局变量用glvs来设置，每个程序运行前均运行一次“glvs”，以设置全局变量（类似于头文件）。作者给出四个常用的坐标系：1. 惯性坐标系i（地球表面）2. 地球坐标系e：地心地固坐标系ECEF3. 导航坐标系n：东北天4. 载体坐标系b：右-前-上Cnb表示从n系到b系的旋转矩阵【注：这一段与前面自己设置的不同】。