三维的组合导航。
ins和卫星的组合导航算法,基于卡尔曼滤波和eskf滤波的都有。
MATLAB源码,有kf和eskf的对比,也有单独的误差,或者输入滤波之后的位移速度等导航参数。
这段程序主要是一个卡尔曼滤波器,用于估计运动物体的位置和速度。它涉及到数据的读取、处理和滤波过程。
首先,程序通过`importdata`函数读取名为`ceshi.txt`的数据文件,并将数据存储在变量`data`中。
接下来,程序从`data`中提取出所需的数据,并将其存储在不同的变量中,如`gvx`、`gvy`、`gvz`等。这些变量代表了运动物体的速度和位置信息。
然后,程序对数据进行处理,计算出一些相关的量,如位移量和时间和。这些处理包括对速度和加速度进行积分,计算位移和时间和。
接下来,程序初始化一些变量,并创建一些空矩阵用于存储测量值和协方差矩阵。
然后,程序进入一个循环,从第二个数据点开始,计算一系列的矩阵和向量,包括转移矩阵、过程噪声协方差、观测矩阵等。然后,程序使用离散卡尔曼滤波公式对状态进行更新和校正,得到估计的位置和速度。
在循环的过程中,程序还计算了卡尔曼滤波器的误差,并将结果存储在一些变量中。
最后,程序绘制了一些图形来展示卡尔曼滤波器的结果,包括估计的位置和速度与测量值的比较,以及误差的分析。
此外,程序还定义了一个名为`kalman`的子函数,用于实现卡尔曼滤波的具体计算过程。该函数接受时间、加速度和GPS测量值作为输入,并返回估计的速度和位置。