卡尔曼滤波器在姿态融合中的推导过程

卡尔曼滤波器针对的问题通常可以使用两个方程来描述:
对于一个随机离散时间过程的状态向量
在这里插入图片描述

其中下标k和k-1分别表示对应的时刻。x是系统状态变量,A是状态转移矩阵或者过程增益矩阵,u是控制输入,B是对应的控制输入增益,w是过程激励噪声,认为是高斯白噪声,期望是0,协方差是Q。
观测方程
在这里插入图片描述

其中z是观测值,v是观测噪声,认为是高斯白噪声,期望是0,协方差是R。
对于以上方程描述的问题,卡尔曼滤波器主要通过5个公式来求得。下面结合陀螺仪加速度计求解欧拉角的问题对公式进行说明。
时间更新方程(预估方程):
第一个方程:
在这里插入图片描述

在本案例中,状态变量是角度和陀螺仪获得的角速度,并且没有其他控制输入变量,因此以上方程可以写成
在这里插入图片描述

第二个方程:
在这里插入图片描述

对于上述状态变量,协方差矩阵的含义为
在这里插入图片描述

为了书写方便,记为
在这里插入图片描述

过程激励噪声矩阵为
在这里插入图片描述

在本案例中,
在这里插入图片描述

对以上方程简化:
在这里插入图片描述

需要注意以上方程右侧第二个矩阵第一项省略二阶无穷小。
测量更新方程(校正方程):
第三个方程:
在这里插入图片描述
在本案例下,观测变量是加速度计获得的角度
其中
在这里插入图片描述
在这里插入图片描述
代入可以得到:
在这里插入图片描述
第4个方程:
在这里插入图片描述
在本案例下,代入可以得到:
在这里插入图片描述
第5个方程:
在这里插入图片描述
在本案例下,代入可以得到:
在这里插入图片描述

以上推导过程和如下博文矛盾:
https://blog.csdn.net/dyl74500196/article/details/59108807

区别在于:我认为状态变量应当是角度和陀螺仪获得的角加速度,而博客上认为状态变量应当是角度和陀螺仪的漂移。究竟哪种说法是正确的?

  • 1
    点赞
  • 6
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值