原创力文档- 1、本文档共5页，可阅读全部内容。
- 2、原创力文档（book118）网站文档一经付费（服务费），不意味着购买了该文档的版权，仅供个人/单位学习、研究之用，不得用于商业用途，未经授权，严禁复制、发行、汇编、翻译或者网络传播等，侵权必究。
- 3、本站所有内容均由合作方或网友上传，本站不对文档的完整性、权威性及其观点立场正确性做任何保证或承诺！文档内容仅供研究参考，付费前请自行鉴别。如您付费，意味着您自己接受本站规则且自行承担风险，本站不退款、不进行额外附加服务；查看《如何避免下载的几个坑》。如果您已付费下载过本站文档，您可以点击 这里二次下载。
- 4、如文档侵犯商业秘密、侵犯著作权、侵犯人身权等，请点击“版权申诉”（推荐），也可以打举报电话：400-050-0827(电话支持时间：9:00-18:30)。
查看更多
卡尔曼滤波器的推导及工作流程
假设AHRS测得的拖拉机三轴姿态角横滚角度、俯仰角度和航向角度分别为：，，，则拖拉机车体坐标系下的空间矢量向大地导航坐标系转换的转换矩阵为：
(1)
假设导航控制点到GPS天线的空间矢量在车体坐标系下的表达形式为， GPS天线在大地导航坐标系下的坐标为，导航控制点在大地导航坐标系下的坐标为，则有：
（2）
The?definition?used?for?'Euler‐angles'?here?is?equivalent?to?'roll,?pitch,?yaw/heading'?(also?known?as?Cardan).?The?Euler‐angles?are?of?XYZ?Earth?fixed?type?(subsequent?rotation?around?global?X,?Y?and?Z?axis,?also?known?as?aerospace?sequence).?From MTi manual.
卡尔曼滤波器的推导及工作流程
设定k时刻拖拉机本体的真实航向角度为，拖拉机本体纵向方向上的真实前进速度是，拖拉机本体纵向方向上的真实加速度是，则
（3）
（）
（）
其中，、、分别指航向角度、前进速度和加速度的测量值，、、分别指航向角度、前进速度和加速度的测量偏差值，、、分别指航向角度、前进速度和加速度的随机测量误差。
基于牛顿第二定律，建立导航控制点 在2D平面坐标系下的运动方程：
将（3）式代入上述表达式，得到
x
y
将上述等式以卡尔曼滤波器状态转移方程的形式表示为：
其中，，表示状态空间向量；
，是状态转移矩阵，由陀螺仪累积航向角度和前进速度的测量值实时更新；
，，是状态转移方程的白噪声序列；系统过程噪声协方差矩阵为，用于表示状态转移方程的误差大小，本发明中设定为常数矩阵，在仿真和实验过程中整定矩阵参数。
以GPS天线在大地导航坐标系下的定位坐标作为观测向量，结合坐标转换公式（1），得到卡尔曼滤波器的测量方程如下：
其中，，，，
将（3）式代入（1）式，
=
，表示OEM GPS板卡定位在水平面坐标系下的随机定位误差。
测量向量的噪声方差矩阵为：
其中，，分别为，的方差统计值。
综合上述推导，采用线性离散卡尔曼滤波器的递归差分方程进行状态向量预测和测量向量校正：
预测方程组为：
校正方程组为：
文档评论（0）