之前提到了IMU与视觉SLAM的融合,因此也避不开卡尔曼滤波的使用。作为最经典的算法之一,推导过程不再赘述,这里只简述一下迭代的公式。
计算预测值,预测值和真实值之间的误差协方差矩阵
有了上式可以计算卡尔曼增益K,然后得到估计值
计算估计值和真实值之间的误差协方差矩阵,为下一次迭代做准备
其中:A为状态转移矩阵,Q为系统噪声方差矩阵,R为观测噪声方差矩阵
之前提到了IMU与视觉SLAM的融合,因此也避不开卡尔曼滤波的使用。作为最经典的算法之一,推导过程不再赘述,这里只简述一下迭代的公式。
计算预测值,预测值和真实值之间的误差协方差矩阵
有了上式可以计算卡尔曼增益K,然后得到估计值
计算估计值和真实值之间的误差协方差矩阵,为下一次迭代做准备
其中:A为状态转移矩阵,Q为系统噪声方差矩阵,R为观测噪声方差矩阵