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

关于Q R的讨论

Q阵

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

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

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

R阵

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

然后对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假设，取各个观测值最大偏差的三倍。尽管这在很多应用中并不合适（比如某些实验不可重复），但有一个准则还是好于任意取值。