不会就学的博客

即一阶马尔可夫假设和高斯噪声假设，那么在其他 KF 的扩充版本中，这两个假设始终存在。对于高斯假设，多传感器融合中各种传感器的噪声类型多样且复杂，并不一定满足高斯噪声假设，因此仍采用该假设会存在一定的问题。其次对于马尔可夫假设，实际上当前状态的影响因素并不是单单只受到上一时刻的状态影响，而可能是之前的很多时刻，那这样的假设会在一定程度上丢失可观测性以及造成误差的累积。尽管如 EKF、UKF 等都是为了解决非线性问题，但实际上都是采用非线性线性化的形式，会存在线性化误差。

存疑的地方：att 应该清空后再用 否则变成了(Att +n*val)/n 了。Cnb组合得到，车体与INS之间不平行存在Cvb, 用Cnb*Cvn 得到Cvb。双天线与车固连且平行，双天线的欧拉角得到Cvn,直接用双天线的得到航向，初始化ins。

imu_tk』工具标定IMU的基本原理IMU误差模型对于加速度计和陀螺仪，定义正交系AOF(GOF)和非正交系AF(GF)：AOF(GOF)系的x轴与AF(GF)系x轴重合AOF(GOF)系的y轴与AF(GF)系的xy平面重合同时，定义正交系BF为载体系，非正交系S为传感器系。陀螺仪和加速度计的常见的指标值有：量程，分辨率，零偏，刻度系数，交叉耦合系数，刻度系数的非线性度，零偏稳定性，零偏重复性，带宽，工作温度等。『imu_tk』工具标定IMU的基本原理_imu标定原理详解-优快云博客。

【代码】ignav的INS的状态更新。

当前位置的重力加速度gn + 滑窗imu的加计三个方向的各自的平均值ym;滑窗口imu的加计三维大小与当前重力差值。静止判断有5种方式，1-4是四种方式，5是前四种的组合判断。滑窗口imu的陀螺三个方向的协方差。滑窗口imu的加计的协方差。

所以这里构建的kalman的量测ZK =hx -v v= hx -zk ，zk为两个方向的量测值0；载体vb : matmul("TN",3,1,3,1.0,Cbe,ve,0.0,vb);nv=bldnhc(opt,imu,ins->Cbe,ins->ve,nx,v,H,R)中构建出来。量测方程中有两个误差项，失准角和e下的速度误差，对应的系数找到。b系下 的待估的误差项为x,

回到INS中，ins中的误差传播是一个非线性的数学模型，那么扰动分析的过程就是将非线性的误差，进行线性化，误差项单独分离出来一阶项，小的误差的情况下，二阶以及高阶忽略掉，也就是最后的分析中，每项误差都独立出来了，线性化了。右边第一项，两个误差耦合在一起，把一阶项独立出来，二阶项忽略掉。扰动分析很重要，搞明白扰动分析，基本上就可以清楚了误差模型。把误差分离出来 误差 = 测量值 - 真值；这里的误差就是与测量直接对应的误差，例如。误差这里称为扰动误差；

同理，标度因数以及交差耦合两项也是系统误差，在给定的常值后，还有一部分误差没有办法给出的，都和零偏一样，需要估计出来，因为也包含常数和变化项，同样也是每次估计都不同。在实际中，一个ins传感器在标定零偏后，使用的输出数据都是减去了标定的零偏的，这部分仅仅是标定后的常值，还有部分误差是没有标定出来的。既有剩余的常值，也有因为温度影响的，或者工作期间的变化项等，每次都不一样，因为含有随时间变化的项，或者随温度变化的项。一个ins数据，在统一扣除了标定后的零偏+温补后，剩余的零偏没有办法标定了的误差，影响精。

不同坐标系下的速度微分公式推导

哪些量需要加扰动误差，在得到一个量的过程中，如果引入了包含误差的量，则需要考虑是否可忽略。首先理解什么是扰动，

{ RTK: 估计的未知数个数是不一样的，紧组合除了模糊度与rtk一致外，还需要各种imu 的相关参数，位置误差，姿态，等等。构成的kalman的v 是用imu的位置 替换到原来的gnss的位置，其他与gnss相似，未知数 位置误差，钟差，姿态，臂杆等，将imu的位置转到给gnss天线，估计钟差。

式子1中 ZK是量测信息， X是待估参数，H则是让两者连接起来的桥梁，直白的理解，就是如何让等式成立。假如量测的方程ZK中的顺序是 Vn系 Pn系，X中的速度误差和位置误差也是n系下，则与之对应的H中的系数就是1；那么需要知道KV和KH，K为增益矩阵，求解方式为3式，可以看出求解K也需要量测矩阵H，所以。h前面已经求出,那么需要知道R，R是量测的方差协方差阵，例如gnss的速度 位置的误差。假如ZK方程有m个，带估参数X有n个，则H矩阵的维数为 m*n;剖析4和5两个方程，

INS中未知数是各类误差，涉及到了误差方程（见下面的公式），Ft就是各个待估参数前面的系数。各个系数都有对应的计算方法，可参考相应的讲义，这里不在一一列举。// 获取状态转移矩阵 离散化后的结果。系统噪声Qk 中用到的是Qt与时间的乘积；时间更新就是利用1、2方程中，k-1时刻的量得到k时刻的状态量。对应方程2的前半部分。若要时间更新，需要知道状态转移矩阵A 以及系统噪声Q；F实际上是状态转移矩阵，其组成就是微分方程的系数。添加系统噪声 对应方程2的后半部分。

有害加速度，一般认为因为其引起的导航坐标系旋转和重力矢量变化都是很小的，所以在计算时间内，选择中间时刻的值代表整个时间段的值，因次可以直接用有害加速度gcc*时间差dt，得到有害加速度产生的速度增量。注意是b系下的比力的速度增量，通过下式，速度增量/时间 == 速度变化率，也就是平均加速度，得到dt时间段内的fb，对比公式和代码，发现公式是用速度增量直接表达，而代码中用的是加速度和时间的关系去表达，所以这里框住的速度增量其实就是。既然是时间相关的更新，对速度微分积分，t-1 ----t的积分。

T/2用的地方有多处，当增量与影响在短时间内其引起的变化比较小的，可以用T/2处的参数等价于整个周期的参数。位置同样是用T时刻的pos，与T+T/2的速度 得到T+T/2的pos，所以地球参数在2T时刻用的是T+T/2的地球参数，然后推到2T,T时刻的vn + an * T/2 其实得到是T+T/2时刻的速度，【T,--T+T/2,---2T】解算时刻为2T时刻，那么地球参数用的是哪一时刻的？假如设计算周期为[T,2T]；那么位置和速度用的哪个时刻的？地球参数相关的更新函数。

四元数的表示，与三角函数之间的关系 ，矢量（x,,y，z） 旋转角度为a， 矢量变化，标量不变,则旋转姿态四元数的表示为。pins中的关于四元数转换 cquat rv2q(const cvect3* rv) 函数。代码对应的公式，第一个。

INS 会受到误差的扰动， INS 的误差方程进行推导。常用的误差模型有 PHI 角和 PSI 角误差模型。PHI 角误差模型是，误差模型易于理解但形式较为复杂。PSI 角误差模型是基于平台坐标系和计算坐标系推导的，误差模型形式简单。固定矢量r 在两坐标系 i 和b 下投影的转换关系，即坐标变换为方程左边r是固定的，所以求微分后为0，右边方程第一项：b系相对于i系的角速率wib, 上标是基准，那么rb的微分就是b系下看r的角速率是负wib，rb的微分就是-wib x rb；所以微分方程就整理成。

为了简单看懂公式，选择简单的参数估计位置、速度、姿态、零偏。前向滤波与后向滤波保持独立，设立两个滤波，互不干扰。前向滤波微分误差方程（具体可以参考严老师的讲义）后向滤波微分误差方程，微分方程。

反向平滑结果 = 正向滤波后更新的结果 + 状态转移矩阵*（前一时刻反向平滑的结果（倒着平滑，这里按照时间是靠后的一个时间） - 对应时刻正向滤波的预测值）。首先进行前向 Kalman 滤波解算，并存储每个时刻的状态更新量以及相应的方差矩阵、状态转移矩阵和状态方差矩阵的预测值；在到达前向滤波末尾时，开始进行反向平滑，反向平滑的初始参数就是正向滤波的最后一个时刻的参数。

2、采集的数据，进行速度解算， 1中的直线行驶默认是没有侧向以及垂向的跳跃的，求解组合导航，得到b系下的解算速度。1、每次安装完成后，选择开阔的环境尽可能的直线行驶，5-10min，需要保证完成初始化后能采集到数据；5、标定完成后，将标定值传入算法，先进行调平，再进行组合导航解算。3、安装偏差角为小角的前提下，解算公式如下，俯仰角和航向角。4、测试时间段的数据统计出来均值，作为最终的标定值。IMU倾斜安装的工程化处理方法，

IMU/GNSS组合导航