// Set observation noise variance and innovation consistency check gate size for the NE position observations
R[0] = _velObsVarNE(0);
R[1] = _velObsVarNE(1);
gate_size[1] = gate_size[0] = _hvelInnovGate;
--------------------------------------------------------------------------------
Vector2f _velObsVarNE; ///< 1-STD observation noise variance used for the fusion of NE velocity data (m/sec)**2
--------------------------------------------------------------------------------
_velObsVarNE(1) = _velObsVarNE(0) = sq(fmaxf(_gps_sample_delayed.sacc, _params.gps_vel_noise));
/// _gps_sample_delayed.sacc是时间范围内的测量值样本 速度误差的1个标准差;gps_vel_noise=0.5m/sgps速度融合的最小允许观测噪声
if (_fuse_vert_vel) {
fuse_map[2] = true;
// observation variance - use receiver reported accuracy with parameter setting the minimum value
R[2] = fmaxf(_params.gps_vel_noise, 0.01f);
// use scaled horizontal speed accuracy assuming typical ratio of VDOP/HDOP
R[2] = 1.5f * fmaxf(R[2], _gps_sample_delayed.sacc);
R[2] = R[2] * R[2];
// innovation gate size
gate_size[2] = fmaxf(_params.vel_innov_gate, 1.0f);
}
if (_fuse_pos) {
// enable fusion for the NE position axes
fuse_map[3] = fuse_map[4] = true;
// Set observation noise variance and innovation consistency check gate size for the NE position observations
R[4] = R[3] = sq(_posObsNoiseNE);
gate_size[4] = gate_size[3] = _posInnovGateNE;
}
上图中gps_sample_delayed由下图获取
获取gps_sample数据
然后对GPS数据进行处理,之后更新buffer
数据缓冲区,buffer给gps sample
所以R中的数据,是取 参数param 还是 测量sample,看fmaxf判断。
如果P0、Q、R无法精确获得,只知道可能的取值范围,则采用可能的较大值(保守)。如果不确切知道Q、R、P0的准确先验信息,应适当增大Q的取值,以增大对实时量测值的利用权重,俗称调谐。但是调谐存在盲目性,无法知道Q要调到多大才行。 ——《卡尔曼滤波与组合导航原理》秦永元 针对 q,根据经典教材Estimation with Applications to Tracking and Navigation一书所讲,通过最高阶状态量对其他量的扰动来初始化,比如位置,速度和加速度三个状态量,就用加速度变化对另外两个量的扰动来初始化,最后得到的是一个 3*3矩阵。 针对 r,根据Gaussian假设,取各个观测值最大偏差的三倍。尽管这在很多应用中并不合适(比如某些实验不可重复),但有一个准则还是好于任意取值。
P阵
所以整个过程,由IMU测量结果进行一步预测Xk/k-1 Pk/k-1 GPS等测量结果进行量测更新Zk Kk Pk Xk