卡尔曼滤波器原理及应用介绍.ppt

单击此处编辑母版标题样式,编辑母版文本样式,第二级,第三级,第四级,第五级,*,.,*,卡尔曼滤波器原理及应用介绍,王文科,2018/11/01,1,.,矩阵运算简介,矩阵加减：,同型矩阵才能进行加减运算，运算时各个对应元素相加减，运算满足结合律、交换律。,矩阵相乘：,例如,C=AB,，,C,的行数与,A,行数相同，,C,的列数与,B,列数相同，,C,的第,i,行,j,列的元素由,A,的第,i,行与,B,的第,j,列对应相乘。如,A,是,m,x,n,矩阵,B,是,n,x,p,矩阵，那么,C,是,m,x,p,矩阵。不满足交换律。,矩阵转置：,将矩阵,A,的行换成同序号的列得到的新矩阵称为,A,的转置矩阵。,单位矩阵：,如同,数的乘法中的,1,。,它是个方阵，从左上角到右下角的对角线（称为主对角线）上的元素均为,1,。除此以外全都为,0,。卡尔曼滤波更新公式中的,I,即指单位矩阵。,两矩阵相加减：,两矩阵相乘：,矩阵转置：,2,.,方差、标准差与协方差,方差,：,方差,用来计算每一个变量（观察值）与总体均数之间的差异。为避免出现离均差总和为零，离均差平方和受样本含量的影响，统计学采用平均离均差平方和来描述变量的变异,程度。,标准差：,对方差开平方。,协方差,：,在,概率论和统计学中用于衡量两个变量的总体误差,。而方差是协方差的一种特殊情况，即当两个变量是相同的情况。如果,两个变量的变化趋势一致，也就是说如果其中一个大于自身的期望值，另外一个也大于自身的期望值，那么两个变量之间的协方差就是正值。,反之，两,个变量之间的协方差就是负值。,方差的两个公式：,协方差公式：,正态分布与标准差：,方差为协方差的特殊情况：,注：使用部分样本统计被测对象的方差时，为了达到无偏估计，使用,(n-1),作为分母。,3,.,协方差矩阵,协方差矩阵：,协方差,矩阵的每个元素是各个向量元素之间的协方差，是从标量随机变量到高维度随机向量的自然,推广。,4,.,卡尔曼滤波器简介,卡尔曼滤波器是,一种高效率的递归,滤波器，,它能够从一系列的不完全及包含噪声的测量中，估计动态系统的状态,。得名,自主要贡献者之一的鲁道夫,卡尔曼（匈牙利裔美国,数学家）。,对于一个线性系统，卡尔曼滤波器能够从不精确的预测状态和观测状态中，估算出高精度的系统状态，并且估计过程只需要保留最近一次的估算结果，具有速度快、资源需求低的特点。,其,滤波过程,为：根据当前状态和系统方程估算下一状态,获取,下一状态的观测结果,使用当前卡尔曼增益加权平均更新估计值,更新卡尔曼增益。整个过程迭代执行。,更新估计值时预测值和观测值所占权重由其不确定性决定，,基本,卡尔曼滤波器擅长处理正态分布的误差。,卡尔曼滤波,包含噪声的,预测状态,包含噪声的,观测状态,接近真实状态的结果,5,.,卡尔曼滤波器公式介绍,卡尔曼滤波器有,5,个基本公式其中,2,个为预测公式，其余,3,个为更新公式。,F,：状态转移矩阵；,B,：控制矩阵；,P,：表示系统不确定性的协方差矩阵；,Q,：表示,预测值,的协方差矩阵；,R,：表示测量值的协方差矩阵；,H,：,系统,状态,到观测状态的变换矩阵；,K,：卡尔曼增益；,P,会快速迭代，初始值选取对滤波效果影响很小；,Q,一般是对角阵，且对角线上的值很,小，较难确定；,R,是,一个数值,，是,和仪器相关的一个特性，作为已知条件输入,滤波器。,预测公式,更新公式,表示该值为估计值,-,表示该状态根据上一状态推测,状态预测公式,不确定性转移公式,实际观察值与预估的观测值之间的残差,6,.,应用举例,-,室内温度估算,卡尔曼滤波器运用的一个简单例子是用于测试一个房间的温度值，假设房间温度在观测过程中是恒定的，同时每过单位时间用温度计测量房间温度，预测和测量结果都存在误差，假设其为正态分布。,在,t-1,时刻的最优值为,23,，该温度的偏差为,3,；,t,时刻的预测偏差为,4,，,t,时刻温度计测得温度,25,，其偏差为,4,。求解,t,时刻房间温度的最优值。,根据上一时刻温度预测该时刻温度不变为,23,该问题下协方差与方差相同，由标准偏差求得方差,计算卡尔曼系数,计算出,t,时刻温度最优值为,24.22,更新,t,时刻温度最优值方差为,9.75,，其标准差为,3.12,该问题状态只有温度一个量，且预测温度不变，卡尔曼滤波公式简化为：,求解过程：,7,.,应用举例,-,小车状态估算,有一做匀速直线运动的小车，状态包含位置,p,和速度,v,，每经过时间,t,使用测距仪测量小车位置。使用卡尔曼滤波器预测车辆的位置和速度，驾驶初始状态时小车的位置和速度向量不具有相关性，,p,和,v,的协方差为,0,；,该系统存在预测协方差矩阵,Q,、测量协方差矩阵,R,、系统协方差矩阵,P,（持续迭代）；,可以得出,F,、,P,、,Q,、,H,、,R,的基本形式如图所示；,小车为匀速运动，不存在控制矩阵，公式简化为：,求解过程：,8,.,应用举例,-,小车状态估算,Matlab,模拟,小车位置设定为,1,200,，时间步长为,1,，状态初始值给,0;0,，位置观测值叠加方差为,1,的高斯噪声；,给出假定的预测协方差矩阵,Q,、观测噪声方差,R,；,滤波结果如图所示，滤波值很快收敛到真实速度,1,附近。,滤波结果,9,.,卡尔曼滤波器参数特性,P,的初始参数对卡尔曼滤波效果影响不大，但,P0/(Q+R),会影响滤波结果的收敛速度；,如,图，分别使用,1 0;0 1,和,2 2;2 2,作为,P,的初始值，经过,30,次迭代，滤波结果就已经基本相同；两种初始,P,值经过迭代后，最终均为,0.1322 0.0093;0.0093 0.0014,。,10,.,卡尔曼滤波器参数特性,Q/(Q+R),的,值代表卡尔曼增益,的收敛,值,，,卡尔曼增益,越,大，说明,测量值越,可靠，最优化结果越,接近测量,值；卡尔曼增益越小，说明预测值,越,可靠，最优化结果越,接近测量,值；,如,图，较大的,Q,参数表示预测值可信度低，滤波结果最终接近观测值（观测值加入了偏差为,1,的高斯噪声，误差较大）；,11,.,卡尔曼滤波器参数特性,R,参数是测量值的协方差矩阵，用于表示测量数据的误差，单一测量结果的,R,参数是一个数值，该值的大小由测量设备本身决定；,R,值的大小会影响卡尔曼滤波的收敛速度和最终滤波精度；,12,.,