卡尔曼滤波算法.docx
- 文档编号:2194220
- 上传时间:2022-10-27
- 格式:DOCX
- 页数:8
- 大小:19.46KB
卡尔曼滤波算法.docx
《卡尔曼滤波算法.docx》由会员分享,可在线阅读,更多相关《卡尔曼滤波算法.docx(8页珍藏版)》请在冰豆网上搜索。
卡尔曼滤波算法
卡尔曼滤波器的介绍
为了可以更加容易的理解卡尔曼滤波器,这里会应用形象的描述方法来讲解,而不是像大多数参考书那样罗列一大堆的数学公式和数学符号。
但是,他的5条公式是其核心内容。
结合现代的计算机,其实卡尔曼的程序相当的简单,只要你理解了他的那5条公式。
在介绍他的5条公式之前,先让我们来根据下面的例子一步一步的探索。
假设我们要研究的对象是一个房间的温度。
根据你的经验判断,这个房间的温度是恒定的,也就是下
一分钟的温度等于现在这一分钟的温度(假设我们用一分钟来做时间单位)
。
假设你对你的经验不是100%的相信,可能会有上下偏差几度。
我们把这些偏差看成是高斯白噪声,也就是这些偏差跟前后时间是没有关系的而且符合高斯分配。
另外,我们在房间里放一个温度计,但是这个温度计也不准确的,测量值会比实际值偏差。
我们也把这些偏差看成是高斯白噪声。
好了,现在对于某一分钟我们有两个有关于该房间的温度值:
你根据经验的预测值(系统的预测值)和温度计的值(测量值)。
下面我们要用这两个值结合他们各自的噪声来估算出房间的实际温度值。
假如我们要估算k时刻的是实际温度值。
首先你要根据k-1时刻的温度值,来预测k时刻的温度。
因为你相信温度是恒定的,所以你会得到k时刻的温度预测值是跟k-1时刻一样的,假设是23度,同时该值的高斯噪声的偏差是5度(5是这样得到的:
如果k-1时刻估算出的最优温度值的偏差是3,你对自己预测的不确定度是4度,他们平方相加再开方,就是5)。
然后,你从温度计那里得到了k时刻的温度值,假设是25
度,同时该值的偏差是4度。
由于我们用于估算k时刻的实际温度有两个温度值,分别是23度和25度。
究竟实际温度是多少呢?
相信自己还是相信温度计呢?
究竟相信谁多一点,我们可以用他们的covariance来判断。
因为Kg^2=5^2/(5^2+4^2)所以Kg=0.78,我们可以估算出k时刻的实际温度值是:
23+0.78*(25-23)=24.56度。
可以看出,因为温度计的covariance比较小(比较相信温度计),所以估算出的最优温度值偏向温度计的值。
现在我们已经得到k时刻的最优温度值了,下一步就是要进入k+1时刻,进行新的最优估算。
到现
在为止,好像还没看到什么自回归的东西出现。
对了,在进入k+1时刻之前,我们还要算出k时刻那个最优值(24.56度)的偏差。
算法如下:
((1-Kg)*5^2)^0.5=2.35。
这里的5就是上面的k时刻你预测的那个23度温度值的偏差,得出的2.35就是进入k+1时刻以后k时刻估算出的最优温度值的偏差(对应于上面的3)。
就是这样,卡尔曼滤波器就不断的把covariance递归,从而估算出最优的温度值。
他运行的很快,而且它只保留了上一时刻的covariance。
上面的Kg,就是卡尔曼增益(KalmanGain)。
他可以随不同的时刻而改变他自己的值,是不是很神奇!
下面就要言归正传,讨论真正工程系统上的卡尔曼。
卡尔曼滤波器算法
在这一部分,我们就来描述源于DrKalman的卡尔曼滤波器。
下面的描述,会涉及一些基本的概念知识,包括概率随即变量高斯或正态分配还有State-spaceModel等等。
但对于卡尔曼滤波器的详细证明,这里不能一一描述。
首先,我们先要引入一个离散控制过程的系统。
该系统可用一个线性随机微分方程(LinearStochasticDifferenceequation)来描述:
X(k)=AX(k-1)+BU(k)+W(k)
再加上系统的测量值:
Z(k)=HX(k)+V(k)
上两式子中,X(k)是k时刻的系统状态,U(k)是k时刻对系统的控制量。
A和B是系统参数,对于
多模型系统,他们为矩阵。
Z(k)是k时刻的测量值,H是测量系统的参数,对于多测量系统,H
为矩阵。
W(k)和V(k)分别表示过程和测量的噪声。
他们被假设成高斯白噪声(WhiteGaussianNoise),他们的covariance分别是Q,R(这里我们假设他们不随系统状态变化而变化)。
对于满足上面的条件(线性随机微分系统,过程和测量都是高斯白噪声),卡尔曼滤波器是最优的信息处理器。
下面我们来用他们结合他们的covariances来估算系统的最优化输出(类似上一节那个温度的例子)。
首先我们要利用系统的过程模型,来预测下一状态的系统。
假设现在的系统状态是k,根据系统的模
型,可以基于系统的上一状态而预测出现在状态:
X(k|k-1)=AX(k-1|k-1)+BU(k)………..
(1)
式
(1)中,X(k|k-1)是利用上一状态预测的结果,X(k-1|k-1)是上一状态最优的结果,U(k)为现在状态的控制量,如果没有控制量,它可以为0。
到现在为止,我们的系统结果已经更新了,可是,对应于X(k|k-1)的covariance还没更新。
我们用P表示covariance:
P(k|k-1)=AP(k-1|k-1)A’+Q………
(2)
式
(2)中,P(k|k-1)是X(k|k-1)对应的covariance,P(k-1|k-1)是X(k-1|k-1)对应的covariance,A’表示A
的转置矩阵,Q是系统过程的covariance。
式子1,2就是卡尔曼滤波器5个公式当中的前两个,也就是对系统的预测。
现在我们有了现在状态的预测结果,然后我们再收集现在状态的测量值。
结合预测值和测量值,我们
可以得到现在状态(k)的最优化估算值X(k|k):
X(k|k)=X(k|k-1)+Kg(k)(Z(k)-HX(k|k-1))………(3)
其中Kg为卡尔曼增益(KalmanGain):
Kg(k)=P(k|k-1)H’/(HP(k|k-1)H’+R)………(4)
到现在为止,我们已经得到了k状态下最优的估算值X(k|k)。
但是为了要另卡尔曼滤波器不断的运行下去直到系统过程结束,我们还要更新k状态下X(k|k)的covariance:
P(k|k)=(I-Kg(k)H)P(k|k-1)………(5)
其中I为1的矩阵,对于单模型单测量,I=1。
当系统进入k+1状态时,P(k|k)就是式子
(2)的P(k-1|k-1)。
这样,算法就可以自回归的运算下去。
使用C语言编程实现(核心算法)。
程序1
x_mid=x_last;//x_last=x(k-1|k-1),x_mid=x(k|k-1)
p_mid=p_last+Q;//p_mid=p(k|k-1),p_last=p(k-1|k-1),Q=噪声
kg=p_mid/(p_mid+R);//kg为kalmanfilter,R为噪声
z_measure=z_real+frand()*0.03;//测量值
x_now=x_mid+kg*(z_measure-x_mid);//估计出的最优值
p_now=(1-kg)*p_mid;//最优值对应的covariance
p_last=p_now;//更新covariance值
x_last=x_now;//更新系统状态值
#include"stdio.h"
#include"stdlib.h"
#include"math.h"
doublefrand()
{return2*((rand()/(double)RAND_MAX)-0.5);//随机噪声}
voidmain()
{
floatx_last=0;
floatp_last=0.02;
floatQ=0.018;
floatR=0.542;
floatkg;
floatx_mid;
floatx_now;
floatp_mid;
floatp_now;
floatz_real=0.56;//0.56
floatz_measure;
floatsumerror_kalman=0;
floatsumerror_measure=0;
inti;
x_last=z_real+frand()*0.03;
x_mid=x_last;
for(i=0;i<20;i++)
{
x_mid=x_last;//x_last=x(k-1|k-1),x_mid=x(k|k-1)
p_mid=p_last+Q;//p_mid=p(k|k-1),p_last=p(k-1|k-1),Q=噪声
kg=p_mid/(p_mid+R);//kg为kalmanfilter,R为噪声
z_measure=z_real+frand()*0.03;//测量值
x_now=x_mid+kg*(z_measure-x_mid);//估计出的最优值
p_now=(1-kg)*p_mid;//最优值对应的covariance
printf("Realposition:
%6.3f\n",z_real);//显示真值
printf("Mesauredposition:
%6.3f
[diff:
%.3f]\n",z_measure,fabs(z_real-z_measure));
//显示测量值以及真值与测量值之间的误差
printf("Kalmanposition:
%6.3f[diff:
%.3f]\n",x_now,fabs(z_real-x_now));//显示
kalman估计值以及真值和卡尔曼估计值的误差
sumerror_kalman+=fabs(z_real-x_now);//kalman估计值的累积误差
sumerror_measure+=fabs(z_real-z_measure);//真值与测量值的累积误差
p_last=p_now;//更新covariance值
x_last=x_now;//更新系统状态值
}
printf("总体测量误差:
%f\n",sumerror_measure);//输出测量累积误差
printf("总体卡尔曼滤波误差:
%f\n",sumerror_kalman);//输出kalman累积误差
printf("卡尔曼误差所占比例:
%d%%\n",100-(int)((sumerror_kalman/sumerror_measure)*100));
}
其实作为应用我们只需知道卡尔曼输入的两个量,一个是测量值,一个是预测值,程序都是成型的,重点还是在参数的调试上。
整个算法中影响输出的就是Kg的值,可以简单的理解为一种加权行为,相信谁更多一点而已。
代码如下:
说明:
简化版卡尔曼滤波
程序2
volatilefloatQingJiao=0; //最终准确角度输出变量定义
volatilefloatGyro_Data=0;//陀螺仪
floatQ=1,R=3900; //调整卡尔曼的滞后3900
staticfloatRealData=0,RealData_P=10000;
floatNowData=0,NowData_P=0;
floatKg=0,gyroscope_rate=0,gyroscope_rat=0,accelerometer_angle;
volatilefloatgyroscope_angle=0 ; //用卡尔曼滤波时不用此变量
int Gyro1_zero=0;
voidkalman_update(void)
{
if(zeroflag>1000) //与开机自检有关,没用到的可以删去
- 配套讲稿:
如PPT文件的首页显示word图标,表示该PPT已包含配套word讲稿。双击word图标可打开word文档。
- 特殊限制:
部分文档作品中含有的国旗、国徽等图片,仅作为作品整体效果示例展示,禁止商用。设计者仅对作品中独创性部分享有著作权。
- 关 键 词:
- 卡尔 滤波 算法