注册 登录  
 加关注
查看详情
   显示下一条  |  关闭
温馨提示!由于新浪微博认证机制调整,您的新浪微博帐号绑定已过期,请重新绑定!立即重新绑定新浪微博》  |  关闭

极夜.潜的博客

 
 
 

日志

 
 

卡尔曼滤波器(KF,Kalman Filter)简介——以MATLAB程序为例  

2011-05-30 20:10:48|  分类: Research |  标签: |举报 |字号 订阅

  下载LOFTER 我的照片书  |

KF在控制、信号和图像处理、雷达、声纳、金融模型等方面有广泛的应用。作为递归滤波器,它通过一系列不完全或是有噪声的数据估计线性动力系统的状态。KF以滤波器状态空间为基础,利用存贮在状态向量中的变量描述系统的全部行为。通过状态转移矩阵和噪声估计对状态向量进行线性和递归式的更新。

KF的基本思想是:采用信号和噪声的状态空间模型,利用前一时刻状态和当前时刻的测量值,估计系统当前时刻的状态。所谓滤波,就是根据当前的量测值和前一时刻系统状态消除随机干扰对测量的影响,提高系统的测量准确度,或者说,根据系统的量测值从被污染的系统中恢复系统的本来面目。系统状态转移的物理规律已知,理论上可以通过状态转移矩阵从前一时刻的状态计算当前时刻的状态,滤波存在的必要性是由于系统存在测量噪声。

KF以“预测—更新”的顺序递推交替进行。预测是利用前一时刻的状态估计当前时刻的状态,这一状态通常又称为先验状态(此时的状态并不包含当前时刻的测量值);更新是利用当前的测量值更新当前状态的估计值,也称为后验状态估计。特别的,如果某一时刻的值不可测量,可不进行更新过程,只进行预测;如果某一时刻测量到了多个值,可进行多次更新操作。下一时刻,继续进行“预测—更新”操作。

一、KF的输入
以测量运动物体的位置为例说明。状态向量x_est包含的变量有位置(x, y)、速度(Vx, Vy)、加速度(Ax, Ay),其中直接测量的变量是位置(x, y),不进行直接测量的隐含变量是速度(Vx, Vy)和加速度(Ax, Ay)。x_est = [x, y, Vx, Vy, Ax, Ay]。
A——状态转移矩阵。由系统状态转移的物理规律决定,通过该矩阵系统状态由前一时刻向当前时刻转换(尚不考虑噪声影响)。根据物体的运动规律
  卡尔曼滤波器(KF,Kalman Filter)简介——以MATLAB程序为例 - 极夜.潜 - 极夜.潜的博客
可知,状态转移矩阵A=[1 0 dt 0 0 0; 0 1 0 dt 0 0; 0 0 1 0 dt 0; 0 0 0 1 0 dt; 0 0 0 0 1 0; 0 0 0 0 0 1]。当前时刻的[x, y, Vx, Vy, Ax, Ay] = A × 前一时刻的[x, y, Vx, Vy, Ax, Ay]。
H——测量矩阵。系统状态中部分变量是可以直接测量的,其它变量是不直接测量的隐含变量。KF主要任务是在存在测量噪声的情况下,利用数值方法,提高可测量变量的精度。H就是从系统状态中分离出可测量变量的矩阵。由于可测变量是位置(x, y),因此H = [1 0 0 0 0 0; 0 1 0 0 0 0]。最终经过KF的测量输出y = H × [x, y, Vx, Vy, Ax, Ay]。
Q——过程噪声协方差。
R——测量噪声协方差。
KF在实际应用中通常难以对噪声的协方差进行良好的估计,MATLAB和Octave可使用ALS(Autocovariance Least-Squares)计算噪声的协方差矩阵。

二、以MATLAB代码为例,说明KF的具体步骤
代码来源于MATLAB自带的demo,可通过coderdemo_setup('coderdemo_kalman_filter')安装。

%   Copyright 2010 The MathWorks, Inc.function y = kalmanfilter(z)  % z是可测变量的当前测量值%% (0)初始化dt=1;% 状态转移矩阵A=[ 1 0 dt 0 0 0;...     % [x ]                0 1 0 dt 0 0;...     % [y ]    0 0 1 0 dt 0;...     % [Vx]    0 0 0 1 0 dt;...     % [Vy]    0 0 0 0 1 0 ;...     % [Ax]    0 0 0 0 0 1 ];       % [Ay]H = [ 1 0 0 0 0 0; 0 1 0 0 0 0 ];    % 测量矩阵% 噪声协方差Q = eye(6);R = 1000 * eye(2);% x_est是系统状态(后验状态)% p_est后验估计误差的协方差persistent x_est p_est                if isempty(x_est)    x_est = zeros(6, 1);  % x_est=[x,y,Vx,Vy,Ax,Ay]'    p_est = zeros(6, 6);end%% (1)预测x_prd = A * x_est; % 先验状态p_prd = A * p_est * A' + Q; % 先验估计误差的协方差%% (1.5)估计Kalman增益矩阵S = H * p_prd' * H' + R;B = H * p_prd';klm_gain = (S \ B)';%% (2)更新% 利用当前测量值、Kalman增益、后验估计、观测矩阵更新系统状态% 更新过程是一个以Kalman增益为权重的加权修正过程x_est = x_prd + klm_gain * (z - H * x_prd);p_est = p_prd - klm_gain * H * p_prd;%% (3)通过KF的测量输出y = H * x_est;end                % of the function

关于程序的几点说明:
1、Kalman增益通过最小均方误差为估计的最佳准则计算。
2、Kalman增益可视为更新的权重矩阵。
3、如果某一时刻的值不可测量,不进行更新操作,y = H × x_prd。
4、本程序与Greg Welch and Gary Bishop的文档An Introduction to the Kalman Filter描述一致。本程序与文档中变量对应关系是:x_est=\hat{x}_k, p_est=P_k, x_prd=\hat{x}_k^-, p_prd=P_k^-。

三、关于KF
1、KF的实质是由量测值重构系统的状态向量,以最小均方误差为估计的最佳准则,寻求一套递推估计的算法,适合于实时处理和计算机运算。
2、如果所有噪声满足正态分布,KF满足误差的均方误差最小。
3、KF是用于线性动力学系统中状态预测的算法,它是类似于隐马尔可夫模型(HMM,Hidden Markov Model)的贝叶斯模型(Bayesian Model),但它的隐含变量的状态空间是连续的,而HMM的是离散的。此外,HMM描述的状态可以是任意形式的分布,而KF采用的是Gaussian噪声模型。
4、KF通过系统的动力学模型、初始条件和测量值(利用传感器)对系统状态进行估计,可视为一种传感器融合算法。

  评论这张
 
阅读(1327)| 评论(0)
推荐 转载

历史上的今天

评论

<#--最新日志,群博日志--> <#--推荐日志--> <#--引用记录--> <#--博主推荐--> <#--随机阅读--> <#--首页推荐--> <#--历史上的今天--> <#--被推荐日志--> <#--上一篇,下一篇--> <#-- 热度 --> <#-- 网易新闻广告 --> <#--右边模块结构--> <#--评论模块结构--> <#--引用模块结构--> <#--博主发起的投票-->
 
 
 
 
 
 
 
 
 
 
 
 
 
 

页脚

网易公司版权所有 ©1997-2018