Kalman滤波简介


  Kalman滤波是一种线性滤波与预测方法,原文为:A New Approach to Linear Filtering and Prediction Problems。文章推导很复杂,看了一半就看不下去了,既然不能透彻理解其原理,但总可以通过实验来理解其具体的使用方法。

  Kalman滤波分为2个步骤,预测(predict)和校正(correct)。预测是基于上一时刻状态估计当前时刻状态,而校正则是综合当前时刻的估计状态与观测状态,估计出最优的状态。预测与校正的过程如下:

  预测:

  校正:

  公式1是状态预测,公式2是误差矩阵预测,公式3是kalman增益计算,公式4是状态校正,其输出即是最终的kalman滤波结果,公式5是误差矩阵更新。各变量说明如下表:

xk

k时刻的状态

A

状态转移矩阵,和具体的线性系统相关

uk

K时刻外界对系统的作用

B

输入控制矩阵,外界的影响如何转化为对状态的影响

P

误差矩阵

Q

预测噪声协方差矩阵

R

测量噪声协方差矩阵

H

观测矩阵

Kk

K时刻的kalman增益

zk

K时刻的观测值

算法实现与分析


  Kalman滤波最复杂的计算应该就是公式3中的矩阵求逆,考虑到实现的方便性,采用matlab来简单实现,本文主要是分析kalman滤波中各个变量的作用和对滤波结果的影响。具体代码如下:

function filter = Kalman(filter)
%predict
predict_x = filter.A * filter.x + filter.B * filter.u;
filter.P = filter.A * filter.P * filter.A' + filter.Q; %correct
filter.K = filter.P * filter.H' / (filter.H * filter.P * filter.H' + filter.R);
filter.x = predict_x + filter.K * (filter.z - filter.H * predict_x);
filter.P = filter.P - filter.K * filter.H * filter.P;
end

  在matlab中,kalman滤波实际上就是上面那5个公式,而难点却是在测试代码中针对不同问题各个变量的初始化上,下面来逐个分析。

1.建立模型,明确观测量,系统状态以及其转移方程(下面这段公式太多,通过word写好后截图)

2.初始化噪声协方差矩阵

  经过上面一步,只有PQRK四个矩阵还未确定了。显然增益矩阵K是不需要初始化的,P是误差矩阵,初始化可以是一个随机的矩阵或者0,只要经过几次的处理基本上就能调整到正常的水平,因此也就只会影响前面几次的滤波结果。

  Q和R分别是预测和观测状态协方差矩阵,一般可以简单认为系统状态各维之间(即上面的a和b)相互独立,那么Q和R就可以设置为对角阵。而这两个对角线元素的大小将直接影响着滤波结果,若Q的元素远大于R的元素,则预测噪声大,从而更相信观测值,这样可能使得kalman滤波结果与观测值基本一致;反之,则更相信预测,kalman滤波结果会表现得比较规整和平滑;若二者接近,则滤波结果介于前面两者之间,根据实验效果看也缺乏实际使用价值。

  以上几个矩阵确定后,对于状态x,由于0时刻我们没有任何关于该系统的知识,可以使用0时刻的测量值z0来初始x0,预测从k=1开始;也可以初始化-1时刻的状态,当然这个状态实际是未知的,也就可随机取。2种方式都可以,但使用0时刻测量值来初始化状态,可以使得前面几次预测更准确。

3.实验分析

  首先使用下面代码生成一组数据存在z.mat中:

interval = pi/18;
t = 1:interval:100*pi;
len = size(t, 2);
a = t + 4 * (rand(1,len)-0.5);
b = t .* sin(t/10) + 10 * (rand(1,len)-0.5);
z = [a; b];
save('z.mat','z');
plot(z(1,:),z(2,:),'o')

  可以看出其近似为一条振幅不断增大的正弦曲线叠加一个随机噪声。绘制出来如下:

  如果使用上面推导的恒定状态系统模型,代码与实验结果如下:

clear
close all
clc dim_observe = 2; %观测值维数
n = dim_observe; %状态维数,观测状态每个维度都有1个速度,故需乘2
filter.A = eye(n);%[1,0,1,0;0,1,0,1;0,0,1,0;0,0,0,1];
filter.B = 0;
filter.u = 0;
filter.P = eye(n);
filter.K = zeros(n);
filter.H = eye(n);%[1,0,0,0;0,1,0,0]; cQ = 1e-8;
cR = 1e-2;
filter.Q = eye(n) * cQ; %这里简单设置Q和R对角线元素都相等,设为不等亦可
filter.R = eye(dim_observe) * cR; filter.x = zeros(n,1); %初始状态x0 load('z.mat');
figure(1),subplot(2,2,1),
t = 1;
out = [];
for i=1:size(z,2)
filter.z = z(:,i);
filter = Kalman(filter);
plot(filter.x(1),filter.x(2), 'r*');hold on
plot(filter.z(1),filter.z(2), 'bo'); hold on
out=[out filter.x];
% pause(.5)
end figure(1),
str = sprintf('cQ = %e, cR = %e', cQ, cR);
title(str) %画局部放大
subplot(2,2,2),
plot(out(1,:),out(2,:), 'r*');hold on
plot(z(1,:),z(2,:), 'bo'); hold on
axis([120 170 80 200])

  可以看出滤波结果完全滞后于测量数据,其根本原因在于建立的模型存在问题。

  如果采用上面推导的物体运动模型则只需要修改部分代码,主要是矩阵A和H,以及其他矩阵对应的维数,具体如下:

dim_observe = 2;      %观测值维数
n = 2 * dim_observe; %状态维数,观测状态每个维度都有1个速度,故需乘2
filter.A = [1,0,1,0;0,1,0,1;0,0,1,0;0,0,0,1];
filter.B = 0;
filter.u = 0;
filter.P = eye(n);
filter.K = zeros(n);
filter.H = [1,0,0,0;0,1,0,0];

  运行结果如下图,蓝色为观测数据,红色为kalman滤波数据,右侧为局部放大图。可以看出经过滤波后的数据相当平滑,这里Q和R中元素的量级分别为cQ和cR,下图结果可以看到cR比cQ多了6个数量级。

(1)

  增加几组结果用于对比分析,对于的cQ和cR见图的标题。

(2)

(3)

(4)

(5)

(6)

  首先看图1和2,cR与cQ大小均相差了3个数量级,而二者的比值相同,则kalman滤波结果相同。

  再看图2~图6,cR/cQ在不断减小,kalman滤波结果的平滑性也在不断降低,到图5和6中,滤波结果完全和观测值相同,说明此时kalman滤波已经完全相信观测值了。原因在于cR/cQ过小,系统认为预测噪声的方差很大,不值得信赖,而观测值的噪声方差小,可信度高。

总结


  根据上面的实验结果,可以看出Kalman滤波应用中的几个问题:

  1.模型建立的正确性从根本上决定了滤波效果的正确性。

  上面使用物体静止模型进行滤波,结果完全不对,而使用匀速运动模型则能达到较好的效果。从根本上讲,上面的数据也不是匀速运动的,为何结果会基本正确?看看第一个使用静止模型的滤波结果,虽然我们假定了物体是静止的,但由于观测数据的作用,kalman滤波结果也会有相应的运动而不是完全静止,也就是说滤波器在不停地修正这个状态,而在匀速运动模型中,物体的速度我们认为不变,但同样地kalman滤波器也会不停地修正这个速度,滤波器中计算的速度实质的偏离了真实速度的,因此最终也会有相应的偏差,不过这个偏差在我们容许范围内,也就可以大胆使用了。

  如果能确定物体是匀变速直线运动,使用相应带加速度的模型会得到更准确的效果。但是越严格的模型其适用范围也相应越小。

  2.影响滤波结果平滑性的因素是cR/cQ,这个值反映了我们对于预测和观测值的信任程度;其值越大则越相信预测结果,滤波结果平滑性好;反之则越相信观测结果,滤波结果越偏向于观测值。一般我们使用kalman滤波器是为了能平滑数据的波动,因此应尽量保证cR/cQ稍大,上面的测试结果该值在1e4以上数据较为平滑。

http://www.cnblogs.com/jcchen1987/p/4371439.html

http://blog.csdn.net/andrew659/article/details/4818988

http://blog.chinaunix.net/uid-26020768-id-3187769.html

http://blog.csdn.net/gengxt2003/article/details/1528325

目标跟踪之卡尔曼滤波---理解Kalman滤波的使用预测的更多相关文章

  1. 目标跟踪之卡尔曼滤波---理解Kalman滤波的使用

    http://www.cnblogs.com/jcchen1987/p/4371439.html

  2. 理解Kalman滤波的使用

    Kalman滤波简介 Kalman滤波是一种线性滤波与预测方法,原文为:A New Approach to Linear Filtering and Prediction Problems.文章推导很 ...

  3. 终于理解kalman滤波

    2017拜拜啦,怎么过元旦呢?当然是果断呆实验室过... 应该是大二的时候首次听说kalman,一直到今天早上,我一看到其5条"黄金公式",就会找各种理由放弃,看不懂呀...但是研 ...

  4. 目标跟踪之粒子滤波---Opencv实现粒子滤波算法

    目标跟踪学习笔记_2(particle filter初探1) 目标跟踪学习笔记_3(particle filter初探2) 前面2篇博客已经提到当粒子数增加时会内存报错,后面又仔细查了下程序,是代码方 ...

  5. 【目标跟踪】相关滤波算法之MOSSE

    简要 2010年David S. Bolme等人在CVPR上发表了<Visual Object Tracking using Adaptive Correlation Filters>一文 ...

  6. Python Opencv-contrib Camshift&kalman卡尔曼滤波&CSRT算法 目标跟踪实现

    本次课题实现目标跟踪一共用到了三个算法,分别是Camshift.Kalman.CSRT,基于Python语言的Tkinter模块实现GUI与接口设计,项目一共包含三个文件: main.py: # co ...

  7. 目标跟踪之相关滤波:CF及后续改进篇

    一. 何为相关滤波? Correlation Filter 最早应用于信号处理,用来描述两个信号之间的相关性,或者说相似性(有点像早期的概率密度),先来看定义: 对于两个数据 f 和 g,则两个信号的 ...

  8. Video Target Tracking Based on Online Learning—深度学习在目标跟踪中的应用

    摘要 近年来,深度学习方法在物体跟踪领域有不少成功应用,并逐渐在性能上超越传统方法.本文先对现有基于深度学习的目标跟踪算法进行了分类梳理,后续会分篇对各个算法进行详细描述. 看上方给出的3张图片,它们 ...

  9. 卡尔曼滤波(kalman)相关理论以及与HMM、最小二乘法关系

    一.什么是卡尔曼滤波 在雷达目标跟踪中,通常会用到Kalman滤波来形成航迹,以前没有学过机器学习相关知识,学习Kalman时,总感觉公式看完就忘,而且很多东西云里雾里并不能深入理解,最后也就直接套那 ...

随机推荐

  1. Request常用方法

    一.HttpServletRequest介绍 HttpServletRequest对象代表客户端的请求,当客户端通过HTTP协议访问服务器时,HTTP请求头中的所有信息都封装在这个对象中,通过这个对象 ...

  2. LoadRunner监控Tomcat的几种方法

    通过JConsole监控Tomcat 1.打开tomcat5的bin目录中的catalina.bat文件,在头部注释部分的后面加上: set JAVA_OPTS=%JAVA_OPTS% -Dcom.s ...

  3. SS配置出错解决方法

      { "server":"0.0.0.0", "local_address":"127.0.0.1", "l ...

  4. Spring-1 之入门

    (一)简单对象Spring  XML配置说明 使用Spring (Spring 3.0) 实现最简单的类映射以及引用,属性赋值: 1.1.新建类UserModel: 1 2 3 4 5 6 7 8 9 ...

  5. Vue 作用域插槽

    使用场景:复用子组件的slot,又可以使slot的内容不一样. <!DOCTYPE html> <html lang="zh"> <head> ...

  6. 准备你的Adempiere开发环境(1)- 编译

    1. 安装JDK 1.6. 2. 安装Eclipse IDE for Java EE Developers. 3. 导入adempire-360lts: 4. 关闭adempiere-360lts的B ...

  7. js Array.prototype.join.call(arguments,",") 理解

    prototype 属性使您有能力向对象添加属性和方法. join() 方法:把数组的所有元素放入一个字符串.元素通过指定的分隔符进行分隔. call() 方法可以用来代替另一个对象调用一个方法. A ...

  8. Task WaitAll的用法

    var tasklst = new List<Task>(); ; i < urls.Count; i++) { tasklst.Add(Task.Factory.StartNew& ...

  9. core文件无堆栈信息定位的思路

    首先需要介绍一下,阅读此文需要有基本的汇编知识. 1.rbp(基址寄存器),rsp(堆栈寄存器),rip(指令寄存器). 2.程序执行时,没有遇到函数调用时,IP自增长,遇到函数调用时, 需要保存bp ...

  10. VS2015 经常不出现智能提示,代码颜色也没有了

    重置下.开始菜单 -->所有程序-->Visual Studio 文件夹 --> Visual Studio Tools --> Developer Command Promp ...