px4 EKF中Q、R阵设置的思考

关于Q R的讨论

Q阵

，状态转移误差矩阵，代表从Xt-1到Xt过程中，状态转移和真实过程之间的误差，具体其中变量可能是对状态转移有影响的变量，比如在有一些场景下，可能为速度，位移，加速度等。加速度对状态扰动最大，所以用加速度对速度和位移的协方差来初始化Q矩阵，是运动模型和物体真实运动之间的差别噪声,预测噪声、系统噪声


daxVar =(dt*param.prediction.angRateNoise)^2
dvx= deltaVelocity(1)
指定IMU角和速度上的噪声方差

设置加速度计和陀螺仪噪声

constrain函数对噪声大小进行限制，随后噪声用于计算daxVar（ekf matlib中Q阵的输入元素）

px4中EKF Q阵就由加速度计、陀螺仪噪声获取，与下面过程噪声processNoise不一样

constrain函数
**

R阵

**，测量误差矩阵，代表用Xt得到Zt的时候的测量误差。R矩阵会影响卡尔曼增益，R越小，卡尔曼增益就越大，收敛就越快。具体其中参数可能是对观测有影响的变量，比如，IMU输入，GPS输入，里程计运动模型输入等。但是这个矩阵过大和过小都会影响滤波的效果。所以在实际调参的过程中要注意，是传感器本身的误差性能情况，量测过程噪声。如GPS不确定度（标准差等），px4中是观测值方差，是提前设置好的传感器误差以及根据测量值样本获取的误差。

px4中过程噪声processNoise的设置

添加过程噪声（非R阵）

px4 ekf gps 气压计等观测值噪声的设置（如上）


上面是默认设置参数表

上图中

// 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