纯惯导卡尔曼滤波器代码实例解读01

其中使用的采集好的数据贴图，

n行7列，具体n=size（data,1）。

具体代码解析

第一段代码：

首先导入的data就是第一个截图示例的n行7列的数据。
数据导入和预处理中的内容就是简单的导入数据，起名为L和R，即左脚右脚的数据，初始化为n行7列的全0矩阵。。
剔除全0行的数据即是判断5-7列的数据所在的每一行是否包含0，含0，则返回0，不包含0，则返回1，最终返回一个列向量，其中只包含0和1，这一行的意思就是剔除5-7列全为0的行，也就是采集到的无效数据。

如图红框所示。
旋转右脚位置
这里前三行为设置pitch，roll，yaw三个欧拉角参数，我截个图来展示：

在这里我们写一下中文名，防止后面搞混，pitch-俯仰角 roll-翻滚角 yaw-偏航角，
最后一行暂时理解为把欧拉角转化为方向余弦矩阵DCM，晚上找万能学长再了解。

下一组数据

把数据分成左右脚的，（采集的数据是左右脚都穿戴该惯导设备采集而来的，因此需要将数据的这些行单独分出来。）

只看左脚即可

定义t从第一行到最后一行，如果该行数据中的第三列数据对应于第一行的第三个数据，也就是，这个：
不一定是偶数行与奇数行，这组数据里就是14或者34,14是左脚的数据，34是右脚的数据，依次根据这两个数据将每一行分别存入data_L和data_R中，即分开左右脚数据并存入data_L和data_R中。

下一组

43行44行的处理和上面的剔除0行数据是类似的，即：将左右脚的全0行数据分别剔除。
然后47-49行的数据为：把每一行取出来，分别由C（DCM方向余弦矩阵）×每一行数据的转置， 再整体转置从而得到东北天坐标系下的数据。

下一组

这一组的比较复杂，我们先试着理解一下，同样只看左脚即可：

第一行主要是指对左脚数据进行一个差分，即计算相邻两行的差值再组成的数据。
56-58行对应的判断差分得到的diff数组中的第一列数据，即是原始数据data_L中的第四列数据的相邻两行的差值，如果这个差值<0,则+127变为正数，原因晚上问完师兄回来补充。
59-62行的数据对应如果diff数组中某一行的第一列数据不等于1，则进行数据插入，repmat（A,3,4）函数即是将A矩阵为单位，复制为3行4列的矩阵。
图中的三个参数如图：

将第一个参数矩阵为单位复制为diff（t,1）行1列的矩阵。第一个参数的意思是遍历diff数组每一行分别除以diff数据中的该行的第一个参数。形成一个数组insert，插入数组。
然后61行数据表示将insert数组插入在第t行。
最后注意最外边的if条件，整体对这个代码进行一个解释就是：
先差分数据，
再遍历每一行，如果其中的第一列数据为负数，则加127.
如果其中的每一行数据的数据不等于1，则插入一个insert行到该行，这个insert就是所谓的填充跳包用的数据，但是速度是如何求得的，没有得到结论，后续再问师兄。

下一组

86-87行设置前四个数据为1 0 0 0，后面三个数据相当于原始数据的5-7列数据差分得到的数据。
94-95行为将左右脚的差分得到的数据的2到7列全部作为一个整体数据，作为左右脚的速度矩阵。
但是为何这样设置我还没理解到位，晚上请教师兄

下一组

98-101的代码比较容易理解：分别设置左右脚的位置为全0的，和速度矩阵同样大小的矩阵，然后分别设置第一行的数据为原始数据的5-7行也就是data_L和data_R的第一行数据。
第106行设置datasize为左右脚中行数比较少的那个数据，取其短的作为整体的数据大小。
然后将速度矩阵和位置矩阵分别取datasize的行数，再组成对应的矩阵即可。

下一组

对所有的数据进行卡尔曼滤波处理。
代码为：

这些卡尔曼滤波的部分就不再赘述，昨天的博文详细讲解了这个卡尔曼滤波器五大公式，此处只是简单的将公式写出来使用即可，不必细究原理。

最后一步就是画图比较了

这样一个完整的纯惯导卡尔曼滤波的导航代码就实现了，画图的代码就不细讲了，最后贴出整个数据得到的图形：

图中纯惯导的就是原始数据data_L和data_R的所在，分别为绿色和粉色的线所示，
加入卡尔曼滤波器进行矫正之后的为蓝线和红线，基本是重叠在一起的，可以看出这组数据非常到位，基本就时将左右脚进行了一个简单的整合，使整个曲线更加合理，在工程上也就意味着实现了卡尔曼滤波器对惯性导航的校正。