基于四元数的扩展卡尔曼(EKF)滤波器四旋翼姿态解算详解-1.KF介绍
2016-08-19 20:42
471 查看
先说明一下,之前介绍的两种姿态解算算法http://blog.csdn.net/wkdwl/article/details/52119163
互补滤波和入梯度下降姿态解算算法,虽然在姿态解算方面效果还比较不错。但是缺点就是这两种算法并没有把传感器的噪声和系统的噪声考虑在内。因此为了进一步提高算法的精度和性能,提出了卡尔曼滤波的姿态估计算法。
在介绍扩展卡尔曼(EKF)之前,先讲解下卡尔曼滤波(KF)。
因此可以得到KF的模型:
其中
在卡尔曼滤波器中被称作预测模型,表示当前的状态
受前一时刻的状态
和本次的控制量
的影响。A表示状态转移矩阵,是一种线性关系的矩阵。B表示系统噪声驱动阵,B形容的也是一种线性关系。
和
均为满足高斯分布的噪声。
在卡尔曼滤波器中被称作观测模型,表示的也是一种线性关系。该式表明在
时刻,我们可以由两种方式获得当前
时刻的状态估计:
1.是来源于预测模型的预测的先验状态
,根据上一时刻的状态量
和本次的控制量
推导而来的。表示在
时刻来临之前就可以获取这个状态。这个就好像我们四旋翼姿态解算中的陀螺仪微分方程,可以预测下一时刻的姿态。
2.是来源于观测模型,在
时刻的测量值,来反过来更新出的当前状态
。这个就像是我们从IMU传感器中读取测量值后来解算当前的姿态。
所以将两种方式得到的当前状态量融合,得到更加精确的当前状态:
其中
为卡尔曼增益,
为测量余差,表示实际测量值与预测测量值之间的偏移。
,
,
,
)
1:
2:
3:
4:
5:
返回
,
KF的推导中两个重要变量:
,在t时刻的状态估计;
,
在t时刻时误差协方差矩阵;
KF操作包括两个阶段:预测和校正。
预测(时间更新):
(预测状态)
(预测协方差矩阵)
校正(测量更新):
(计算卡尔曼增益)
(通过测量值更新当前的估计状态)
(更新协方差)
的已知量
,
,
,
,求出当前
时刻的预测值
和
计算卡尔曼增益
。
,
,
得到:
所以得到:
这个公式说明了一个KF绝妙之处:
1.如果测量结果无误差,也既
2.如果预测值得协方差矩阵
因此,
下面到了KF中最精华的一步了,现在我们有了对状态的预测值和协方差,同时也收集到了对状态的测量值。这时就可以通过kalman增益来计算状态估计值了。
最后更新出当前状态的协方差矩阵
到这里相信大家基本上KF的流程差不多熟悉了,就是不断计算当前状态和协方差矩阵 来不断迭代。
下面给出框图
由此KF介绍完毕,但是在四旋翼姿态解算中,由于系统并不是线性系统。所以需要扩展卡尔曼滤波算法。在下一篇会讲到。
互补滤波和入梯度下降姿态解算算法,虽然在姿态解算方面效果还比较不错。但是缺点就是这两种算法并没有把传感器的噪声和系统的噪声考虑在内。因此为了进一步提高算法的精度和性能,提出了卡尔曼滤波的姿态估计算法。
在介绍扩展卡尔曼(EKF)之前,先讲解下卡尔曼滤波(KF)。
KF滤波的模型建立以及公式推导
KF滤波器的适用范围:线性系统。下图为KF流程图因此可以得到KF的模型:
其中
在卡尔曼滤波器中被称作预测模型,表示当前的状态
受前一时刻的状态
和本次的控制量
的影响。A表示状态转移矩阵,是一种线性关系的矩阵。B表示系统噪声驱动阵,B形容的也是一种线性关系。
和
均为满足高斯分布的噪声。
在卡尔曼滤波器中被称作观测模型,表示的也是一种线性关系。该式表明在
时刻,我们可以由两种方式获得当前
时刻的状态估计:
1.是来源于预测模型的预测的先验状态
,根据上一时刻的状态量
和本次的控制量
推导而来的。表示在
时刻来临之前就可以获取这个状态。这个就好像我们四旋翼姿态解算中的陀螺仪微分方程,可以预测下一时刻的姿态。
2.是来源于观测模型,在
时刻的测量值,来反过来更新出的当前状态
。这个就像是我们从IMU传感器中读取测量值后来解算当前的姿态。
所以将两种方式得到的当前状态量融合,得到更加精确的当前状态:
其中
为卡尔曼增益,
为测量余差,表示实际测量值与预测测量值之间的偏移。
Kalman Filter五条黄金公式 :
输入量(,
,
,
)
1:
2:
3:
4:
5:
返回
,
KF的推导中两个重要变量:
,在t时刻的状态估计;
,
在t时刻时误差协方差矩阵;
KF操作包括两个阶段:预测和校正。
预测(时间更新):
(预测状态)
(预测协方差矩阵)
校正(测量更新):
(计算卡尔曼增益)
(通过测量值更新当前的估计状态)
(更新协方差)
KF公式推导
首选输入上一时刻的已知量
,
,
,
,求出当前
时刻的预测值
和
计算卡尔曼增益
。
,
,
得到:
所以得到:
这个公式说明了一个KF绝妙之处:
1.如果测量结果无误差,也既
为0的时候,有
,,代入预测模型得到,只剩下了测量部分。
2.如果预测值得协方差矩阵
为0,既说明预测结果无误差,则只剩下预测部分。
因此,
会根据测量值和预测值的信任程度来调节权重。
下面到了KF中最精华的一步了,现在我们有了对状态的预测值和协方差,同时也收集到了对状态的测量值。这时就可以通过kalman增益来计算状态估计值了。
越大表明越相信测量值,反之越相信预测值。
最后更新出当前状态的协方差矩阵到这里相信大家基本上KF的流程差不多熟悉了,就是不断计算当前状态和协方差矩阵 来不断迭代。
下面给出框图
由此KF介绍完毕,但是在四旋翼姿态解算中,由于系统并不是线性系统。所以需要扩展卡尔曼滤波算法。在下一篇会讲到。
相关文章推荐
- 基于四元数的扩展卡尔曼(EKF)滤波器四旋翼姿态解算详解-2.EKF介绍
- 基于四元数的扩展卡尔曼滤波器(EKF)姿态估计文献小结
- 基于四元数的简单互补滤波姿态解算
- 基于四元数的姿态解算算法图解
- 基于EKF的姿态解算
- 基于四元数的姿态解算算法图解
- 卡尔曼(Kalman)滤波(六)--卡尔曼滤波的应用: 四元数卡尔曼滤波(QKF)的C代码实现姿态解算
- Pixhawk之姿态解算篇(5)_ECF/EKF/GD介绍
- 四元数姿态解算中的地磁计融合解读
- 基于PHP扩展一种处理Emoji方法的类库介绍【Carmela】
- 卡尔曼滤波(KF)和扩展卡尔曼滤波(EKF)
- 基于Linux下详解正则表达式(基本正则和扩展正则命令使用实例)
- Pixhawk之姿态解算篇(5)_ECF/EKF/GD介绍
- 四旋翼飞行器 四元数和欧拉角的关系 与 姿态解算
- Solr的创建者介绍Apache Solr : 基于Lucene的可扩展集群搜索服务器
- 飞控姿态解算中,欧拉角与四元数之间的转换
- 四元数---姿态解算
- 四旋翼飞行器 四元数和欧拉角的关系 与 姿态解算
- php_curl.dll扩展介绍和详解
- 基于四元数法的捷联式惯性导航系统的姿态解算(完善中)