【组合导航代码ECL】状态预测函数predictCovariance.m

主函数

function P  = PredictCovariance(deltaAngle, ...
    deltaVelocity, ...
    states,...
    P, ...  % Previous state covariance matrix
    dt, ... % IMU and prediction time step
    param) % tuning parameters

其中变化的角度值与速度值都是经过预测函数改正的

确定过程噪声Q

IMU 系统偏差的过程噪声

${\sigma }_{\Delta Ang\_Bias}=Para(\sigma \_Ang)\ast {dt}^2$
${\sigma }_{\Delta Vel\_Bias}=Para(\sigma \_Vel)\ast {dt}^2$
${\sigma }_{magNED}=Para(magNED)\ast dt$
${\sigma }_{magXYZ}=Para(magXYZ)\ast dt$
$Q_{bias}=zeros(1,10),{\sigma }_{\Delta Ang\_Bias},{\sigma }_{\Delta Vel\_Bias},{\sigma }_{magNED},{\sigma }_{magXYZ},[0,0]$

确定其他过程噪声

IMU 过程噪声${\Delta }_{ang}$${\Delta }_{vel}$

$Va{r}_{{\Delta }_{ang}}=(Para({\sigma }_{angRate})\ast dt{)}^2$
$Va{r}_{{\Delta }_{vel}}=(Para({\sigma }_{accel})\ast dt{)}^2$

计算F状态转移矩阵

通过函数calcF24.m计算，需要传入：变化的速度值、变化的角度值、四元素、速度变化值的系统偏差值与角度变化值的系统偏差值

计算Q（除了IMU系统偏差过程噪声）

通过函数calcQ24.m计算，需要传入：变化的角度值协方差、变化的速度值协方差、四元素

计算$P_k$

$P_k^{-}=F\ast P_{k-1}\ast F^T+Q$
再加上IMU系统偏差噪声
$P_K^{-}=P_k^{-}+Q_{bias}$

强制P阵为对称阵以及协方差>0

小结

1. 函数分开两部分求，系统偏差与其他状态量，系统偏差状态的状态转移矩阵为单位阵，而其他状态量不是，也许这个是分开求的原因？
2. 函数的步骤为：1）通过参数设置中得到系统偏差的噪声值；2）通过参数设置得到速度变化值与角度变化值的协方差，进计算F,Q，通过上一历元的P得到P(K)

疑问

1. IMU过程噪声magXYZ是否是指mag偏差的噪声
2. IMU系统偏差中的各项误差为什么没有平方，标准差没有变成协方差啊