mpu6050姿态解算与卡尔曼滤波(2)卡尔曼滤波

原创 2017年03月09日 12:10:59

卡尔曼滤波,是最优估计理论中十分重要的一个部分。
要全面地理解卡尔曼滤波,你需要一点统计学的知识,以及一点矩阵理论。通常最优估计理论的教材是从最小二乘估计讲起,接着讲到最小方差估计,极大似然估计以及维纳滤波,再到卡尔曼滤波、扩展卡尔曼、无迹卡尔曼——-这是从统计的角度来理解卡尔曼滤波。
事实上,卡尔曼滤波与《自动控制原理》中的状态观测器非常相似。状态观测器利用原系统的输入输出来估计系统状态,对原系统建模(状态方程,输出方程),把观测器的输出与原系统输出的差乘上一个增益反馈给观测器的状态,这样构成一个闭环系统使得观测器状态跟踪原系统状态。卡尔曼滤波中,同样要对系统建模得到状态方程,输出方程则取决于你的测量手段,因此改称量测方程。卡尔曼同样要预测量测值,并把预测值与真实量测值比较,乘上一个增益返回给状态。不同之处在于,状态观测器不考虑噪声,要求系统可检测,并且增益通常是时不变的,而卡尔曼滤波则详细考虑了噪声和初始估计误差,智能的计算每一步的增益,以达到对状态的最优的估计。


实际工程常常面临这样的情况:变量X无法直接测量,能测量的是一些与X相关的量Z,通常可以得到确定的函数关系:Z=H(X),例如要用mpu6050测量转动欧拉角(ψ,θ,γ)T,无法直接测量,但是加速度计的测量值与欧拉角有关系:

axayaz=gsinγcosθgsinθgcosγcosθ

由测量值Z估计状态X,典型的方法–最小二乘估计,即对X的估计—X^应当使得E{(ZH(X^))(ZH(X^))T}最小。典型例子如用直尺测量一支笔的长度,多次测量取平均值就是一个最简单的最小二乘估计。
实际工程中,对状态X可以通过建模,给出X的动力学过程:X˙=f(X,t),若能确定初始时刻的状态X0,那么显然初始时刻以后的状态都可以确定,对状态X的确定就是一个微分方程求解的问题。然而实际系统f(X)多具有复杂的形式,给出X的解析解是一件困难甚至不可能的事。好在我们并不要求X绝对精确的值,只需要满足精度需求的值,那么我们可以依靠数值积分方法给出状态X的解,只要步长Δt取得足够小,龙格库塔法正是其中的一种典型方法。
X的动力学过程与对Z的测量都包含有关于X的信息,卡尔曼滤波正是综合运用了这两种信息来给出状态x的最优估计。
卡尔曼滤波包含两个基本方程–状态方程和量测方程,这也是卡尔曼滤波最根本的地方,但凡要实现一个卡尔曼滤波器,只要写出状态方程和量测方程,剩下的就只是代公式了,如果连状态方程和测量方程都写不出来搞不清楚,那也别谈什么实现卡尔曼滤波器。
状态方程:
Xk=Φk/k1Xk1+Γk1Wk1

状态方程描述了状态X的动力学过程,Φk/k1是从k-1时刻到k时刻的状态转移矩阵,Γk1是k-1时刻的噪声输入矩阵,Wk1是k-1时刻的系统噪声。
量测方程:
Zk=HkXk+Vk

量测方程描述了状态与量测量的关系,Vk是k时刻的测量噪声。
上述两个方程是标准线性卡尔曼的基本方程,它要求两个方程是线性的,还要求系统噪声W和测量噪声V都是白噪声。
卡尔曼的推导过程需要较强的数学基础,这里不讨论,只说明卡尔曼的具体做法和物理意义。
卡尔曼要解决的问题:若在k-1时刻估计状态为X^k1,在k时刻有一个测量Zk,那么k时刻对状态的预测X^k应该是多少呢?
回答这个问题,需要考察状态方程:
Xk=Φk/k1Xk1+Γk1Wk1

和量测方程:
Zk=HkXk+Vk

以及必要的假设:系统噪声W和测量噪声V都是白噪声,即E(Wk)=0E(WkWTj)=QkδkjE(Vk)=0E(VkVTj)=Rkδkj,并且假设k-1时刻状态估计为X^k1,误差协方差为Pk1=E(Xk1X^k1)(Xk1X^k1)T
首先,既然在k-1时刻已经估计出状态为X^k1,那么k时刻的状态按照状态方程就应当预测为:
X^k/k1=Φk/k1X^k1
X^k/k1被称为状态一步预测值。
既然对k时刻状态做出预测,那么显然可以根据量测方程再对k时刻的测量值做出预测:
Z^k=HkX^k/k1

既然对测量做出了预测,又获得了一个真正的测量值Zk,那么显然它们的差ZkZ^k反映了预测状态与真实状态的差。所以毫无疑问,需要利用这个差对状态做出修正:
X^k=X^k/k1+Kk(ZkHkX^k/k1)

Kk是增益阵,那么剩下的问题就是如何选取Kk。选取Kk的原则是使得误差协方差阵 E(XkX^k)(XkX^k)T最小,按照上式,对Kk的选取既要考虑X^k/k1的误差协方差,也要考虑测量的噪声阵Rk,记Pk/k1X^k/k1的误差协方差阵,则Kk为:
Kk=Pk/k1HTk(HkPk/k1HTk+Rk)1

Pk/k1与上一步的误差协方差 Pk1 和系统噪声 Qk1有关:
Pk/k1=Φk/k1Pk1ΦTk/k1+Γk1Qk1ΓTk1

到这里就确定了增益K,给出了估计X^k,最后一步是计算 X^k 的误差协方差Pk
Pk=[IKkHk]TPk/k1[IKkHk]T+KkRkKTk

Pk还有两种形式:
Pk=[IKkHk]Pk/k1
P1k=P1k/k1+HTkR1kHk

以及Kk也还有一种简单形式:
Kk=PkHTkR1k

从上述过程可以看出,卡尔曼滤波遵循“预测-修正”的思想,通过对比测量真实值与测量预测值来修正状态估计。确定增益阵K是卡尔曼滤波的核心也是精髓所在,对增益阵的计算充分考虑了初始估计误差,系统噪声,测量噪声,给出了误差最小意义下的状态最优估计。


再说一下扩展卡尔曼(EKF)。
线性卡尔曼要求状态方程和量测方程都是线性方程,在实际问题中面临最多的却是非线性系统的问题。
对于非线性的状态方程和量测方程:

Xk=f(Xk1,k1)+Γk1Wk1

Zk=h(Xk,k)+Vk

上述的基于线性方程的做法已经不再适用了。为了能对非线性系统应用卡尔曼滤波,利用泰勒展开对原非线性系统在Xk1=X^k1 处做一阶线性化:
Xk=f(X^k1,k1)+Jf(k1)(Xk1X^k1)+Γk1Wk1

Zk=h(Xk,k)+Jh(k)(XkX^k)+Vk

这样就能应用线性方程中的做法进行滤波,这就是扩展卡尔曼滤波。
EKF在Xk=X^k处用一个线性化的方程对原非线性方程做近似,这个做法在系统非线性不强的时候能够给出比较好的估计,但是在非线性较强的时候,EKF很难获得较高的滤波精度。

版权声明:本文为博主原创文章,未经博主允许不得转载。

扩展卡尔曼滤波姿态解算

  • 2009年04月15日 13:15
  • 2.05MB
  • 下载

姿态解算系列一:经验型卡尔曼数据融合

目的:我们需要得到机器人运动的姿态信息,三个轴的角度以及角速度。 本文大纲: 1、传感器相关模型  2、坐标变换    3、卡尔曼数据融合 4、姿态解算流程  5、DMP  6、数据融合效果...
  • haishaoli
  • haishaoli
  • 2016年10月29日 17:02
  • 6157

卡尔曼(Kalman)滤波(六)--卡尔曼滤波的应用: 四元数卡尔曼滤波(QKF)的C代码实现姿态解算

0 引言 在捷联惯导工程实践[6]中,我们希望陀螺仪能够非常精确的获取信息,或者说希望陀螺仪能非常准确的地反映观测量(加速度,磁场等)[6,7]的真实值,但是这个过程或多或少是受到噪声干扰的,导致测...
  • eternity1118_
  • eternity1118_
  • 2016年06月19日 17:08
  • 4439

姿态解算系列一:经验型卡尔曼数据融合

目的:我们需要得到机器人运动的姿态信息,三个轴的角度以及角速度。 本文大纲: 1、传感器相关模型  2、坐标变换    3、经验型卡尔曼数据融合 4、姿态解算流程  5、D...
  • datase
  • datase
  • 2017年07月02日 20:50
  • 352

Pixhawk之姿态解算篇(5)_ECF/EKF/GD介绍

一、开篇         很久没更新blog了, 最近研究的东西比较杂乱,也整理了很多东西,没有来的及更新,最近发现很多小伙伴都开始写blog了,在不更新就要“被落后了”。兄弟们,等等我啊~~~ ...
  • qq_21842557
  • qq_21842557
  • 2016年06月21日 16:07
  • 14629

Arduino uno + mpu6050 陀螺仪 运用卡尔曼滤波姿态解算实验

本例程输出XYZ的角度,感觉算是比较稳定,但好像有点偏移。 首先看看XYZ轴的输出效果图: Bom表 Arduino Uno               *1 mpu6050 陀螺仪模块 *1 ...
  • ling3ye
  • ling3ye
  • 2016年05月10日 09:43
  • 22783

谈谈MPU6050的数据融合 一阶滤波 卡尔曼滤波

首先要明确,MPU6050 是一款姿态传感器,使用它就是为了得到待测物体(如四轴、平衡小车) x、y、z 轴的倾角(俯仰角 Pitch、滚转角 Roll、偏航角 Yaw) 。我们通过 I2C 读取到 ...
  • zsn15702422216
  • zsn15702422216
  • 2016年08月16日 18:17
  • 14107

MPU6050采集+卡尔曼滤波

  • 2014年08月29日 10:33
  • 2.03MB
  • 下载

mpu6050卡尔曼滤波程序及分析

最近在学习卡尔曼滤波算法,算法 首先静止传感器,先测量100次,求平均值,求出偏差Ax_offset   Az_offset   Gz_offset.以后每次测量值 都减去这一偏差。然后通过加速度测得...
  • jasondooc
  • jasondooc
  • 2015年09月24日 18:04
  • 6626

关于MPU6050姿态解算的理解与认识

谨以此纪念两天以来的对电子罗盘的认识与学习    mpu6050常用作提供飞控运行时的姿态测量和计算,在在姿态结算中有几个重要的概念,欧拉角、四元数等。 欧拉角:用来表征三维空间中运动物体绕着坐标轴旋...
  • u013937192
  • u013937192
  • 2016年03月24日 17:05
  • 13845
内容举报
返回顶部
收藏助手
不良信息举报
您举报文章:mpu6050姿态解算与卡尔曼滤波(2)卡尔曼滤波
举报原因:
原因补充:

(最多只允许输入30个字)