国内最全IT社区平台 联系我们 | 收藏本站
华晨云阿里云优惠2
您当前位置:首页 > 互联网 > 卡尔曼滤波器算法浅析及matlab实战

卡尔曼滤波器算法浅析及matlab实战

来源:程序员人生   发布时间:2015-05-12 08:58:49 阅读次数:5793次

卡尔曼滤波器是1种利用线性系统状态方程,通过系统输入输出观测数据,对系统状态进行最优估计的算法。而且由于观测包括系统的噪声和干扰的影响,所以最优估计也可看作是滤波进程。

卡尔曼滤波器的核心内容就是5条公式,计算简单快速,合适用于少许数据的预测和估计。

下面我们用1个例子来讲明1下卡尔曼算法的利用。

假定我们想在有1辆小车,在 t 时刻其速度为 Vt ,位置坐标为 Pt,ut 表示 t 时刻的加速度,那末我们可以用Xt表示 t 时刻的状态,以下:


则我们可以得到,由t⑴ 时刻到 t 时刻,位置和速度的转换以下:


用向量表示上述转换进程,以下:


以下图:


那末我们可以得到以下的状态转移公式:


(1)

其中矩阵 F 为状态转移矩阵,表示如何从上1状态来推测当前时刻的状态,B 为控制矩阵,表示控制量u如何作用于当前矩阵,上面的公式 x 有顶帽子,表示只是估计值,其实不是最优的。

有了状态转移公式就能够用来推测当前的状态,但是所有的推测都是包括噪声的,噪声越大,不肯定越大,协方差矩阵用来表示这次推测带来的不肯定性

协方差矩阵

假定我们有1个1维的数据,这个数据每次丈量都不同,我们假定服从高斯散布,那末我们可以用均值和方差来表示该数据集,我们将该1维数据集投影到坐标轴上,以下图:


可以看到,服从高斯散布的1维数据大部份散布在均值附近。

现在我们来看看服从高斯散布的2维数据投影到坐标轴的情况,以下图:


2维数据比1维数据略微复杂1点,投影后有3种情况,分别是:
左图:两个维的数据互不相干;
中图:两个维的数据正相干,也就是 y 随着 x 的增大而增大(假定两个维分别为 x 和 y)
右图:两个维的数据负相干,也就是 y 随着 x 的增大而减小。

那怎样来表示两个维的数据的相干性呢?答案就是协方差矩阵。


状态协方差矩阵传递

在公式(1)当中,我们已得到了状态的转移公式,但是由上面可知,2维数据的协方差矩阵对描写数据的特点是很重要的,那末我们应当如何更新或说传递我们的2维数据的协方差矩阵呢?假设我们用 P 来表示状态协方差,即


那末加入状态转换矩阵 F ,得到


(2)

也即:


因此我们便得到了协方差的转换公式。

现在我们得到了两个公式,应用这两个公式能够对现在状态进行预测。依照我们的正常思路来理解,预测结果不1定会对嘛,肯定有误差。而且在我们大多数回归算法或是拟合算法中,1般思路都是先预测,然后看看这个预测结果跟实际结果的误差有多大,再根据这个误差来调剂预测函数的参数,不断迭代调剂参数直到预测误差小于1定的阈值。

卡尔曼算法的迭代思想也类似,不过这里根据误差调剂的是状态 X 。

在这里,我们的实际数据就是 Z, 以下图:


其中,矩阵 H 为丈量系统的参数,即视察矩阵,v 为观测噪声, 其协方差矩阵为R

那末我们的状态更新公式以下:


其中K 为卡尔曼系数, Z-Hx 则为残差,也就是我们说的,预测值与实际值的误差。

K的作用:

1.K 权衡预测协方差P和视察协方差矩阵R那个更加重要,相信预测,残差的权重小,相信视察,残差权重大,由 K 的表达是可以退出这个结论
2,将残差的表现情势从视察域转换到状态域(残差与1个标量,通过K 转换为向量),由 状态 X 的更新公式可得到该结论。

至此,我们已得到了 t 状态下的最优估计值 Xt。但为了能让我们的迭代算法延续下去,我们还必须更新状态协方差的值。

状态协方差的更新


以上就是卡尔曼滤波算法的思想,只有简单的 5 条公式,总结以下:


Matlab 实现

function kalmanFiltering %% clc close all %% % % Description : kalmanFiltering % Author : Liulongpo % Time:2015⑷⑵9 16:42:34 % %% Z=(1:2:200); %观测值 汽车的位置 也就是我们要修改的量 noise=randn(1,100); %方差为1的高斯噪声 Z=Z+noise; X=[0 ; 0 ]; %初始状态 P=[1 0;0 1]; %状态协方差矩阵 F=[1 1;0 1]; %状态转移矩阵 Q=[0.0001,0;0 , 0.0001]; %状态转移协方差矩阵 H=[1,0]; %观测矩阵 R=1; %观测噪声方差 figure; hold on; for i = 1:100 %基于上1状态预测当前状态 X_ = F*X; % 更新协方差 Q系统进程的协方差 这两个公式是对系统的预测 P_ = F*P*F'+Q; % 计算卡尔曼增益 K = P_*H'/(H*P_*H'+R); % 得到当前状态的最优化估算值 增益乘以残差 X = X_+K*(Z(i)-H*X_); %更新K状态的协方差 P = (eye(2)-K*H)*P_; scatter(X(1), X(2),4); %画点,横轴表示位置,纵轴表示速度 end end

效果以下


其中 x 轴为位置,y轴为速度。
在代码中,我们设定x的变化是 1:2:200,则速度就是2,可以由上图看到,值经过几次迭代,速度就基本上在 2 附近摆动,摆动的缘由是我们加入了噪声。

接下来来看1个实际例子。

我们的数据为 data = [149.360 , 150.06, 151.44, 152.81,154.19 ,157.72];
这是应用光流法从视频中获得角点的实际x轴坐标,总共有6个数据,也就是代表了1个点的连续6帧的x轴坐标。接下来这个例子,我们将实现用5帧的数据进行训练,然后预测出第6帧的x轴坐标。

在上1个matlab例子中,我们的训练数据比较多,因此我们的初始状态设置为[0,0],也就是位置为0,速度为0,在训练数据比较多的情况下,初始化数据为0并没有关系,由于我们在上面的效果图中可以看到,算法的经太短暂的迭代就可以够发挥作用。

但在这里,我们的训练数据只有5帧,所以为了加快训练,我们将位置状态初始化为第1帧的位置,速度初始化为第2帧与第1帧之差。

代码以下:

KF.m

function [predData,dataX] = KF(dataZ) %% % % Description : kalmanFiltering % Author : Liulongpo % Time:2015⑷⑵9 16:42:34 % %% Z = dataZ'; len = length(Z); %Z=(1:2:200); %观测值 汽车的位置 也就是我们要修改的量 noise=randn(1,len); %方差为1的高斯噪声 dataX = zeros(2,len); Z=Z+noise; X=[Z(1) ; Z(2)-Z(1) ]; %初始状态 分别为 位置 和速度 P=[1 0;0 1]; %状态协方差矩阵 F=[1 1;0 1]; %状态转移矩阵 Q=[0.0001,0;0 , 0.0001]; %状态转移协方差矩阵 H=[1,0]; %观测矩阵 R=1; %观测噪声方差 %figure; %hold on; for i = 1:len %基于上1状态预测当前状态 % 2x1 2x1 X_ = F*X; % 更新协方差 Q系统进程的协方差 这两个公式是对系统的预测 % 2x1 2x1 1x2 2x2 P_ = F*P*F'+Q; % 计算卡尔曼增益 K = P_*H'/(H*P_*H'+R); % 得到当前状态的最优化估算值 增益乘以残差 X = X_+K*(Z(i)-H*X_); %更新K状态的协方差 P = (eye(2)-K*H)*P_; dataX(:,i) = [X(1);X(2)]; %scatter(X(1), X(2),4); %画点,横轴表示位置,纵轴表示速度 end predData = F*X; end

testKF.m

function testKF %% clc close all %% %data = load('D:a.txt'); %data = [149.360 , 150.06, 151.44, 152.81,154.19,157.72,157.47,159.33,153.66]; data = [149.360 , 150.06, 151.44, 152.81,154.19 ,157.72]; [predData , DataX] = KF(data'); error = DataX(1,:) - data; i = 1:length(data); figure subplot 311 scatter(i,data,3),title('原始数据') subplot 312 scatter(i,DataX(1,:),3),title('预测数据') subplot 313 scatter(i,error,3),title('预测误差') predData(1) %{ scatter(i,error,3); figure scatter(i,data,3) figure scatter(i,predData(1,:),3) %} end

效果以下:


预测结果为: 155.7493 ,跟实际结果 157.72 唯一1.9 的误差,可以看到,卡尔曼滤波器算法对少许数据的预测效果还是挺不错的。固然,预测位置的同时,我们也得到了预测速度。

参考文献: 视频: 卡尔曼滤波的原理和在MATLAB中的实现

生活不易,码农辛苦
如果您觉得本网站对您的学习有所帮助,可以手机扫描二维码进行捐赠
程序员人生
------分隔线----------------------------
分享到:
------分隔线----------------------------
关闭
程序员人生