卡尔曼滤波
这个视频《理解卡尔曼滤波器 - understanding Kalman Filters》讲的比较好,
视频链接https://www.bilibili.com/video/av24098897/?p=3
问题分析:很显然,我们可以通过对速度v的积分来获取当前位置,可惜这个值s1还有累计误差,而且当小车打滑、过沟时,我们手里的v的值也就并不是那么准确,这样得出的预测值s1,就不是很可靠。这一过程我们称为:用系统的数学模型来预估。
还有一个获得路程s的方法就是读取GPS的值,减掉小车的起始位置,为了叙述简便,我们直接把这两者的差值叫做:GPS的输出值,由于GPS的输出值是有噪声的,GPS读出的测量值S2,也不是很可靠。这一过程我们称为:用测量值来预估。
控制器的计算过程只能是离散的,无法使用积分,只能使用累加:假设我们每20ms计算一次,那么预测值:
s1(k)=s1(k-1) + Bv + w
式中,B为采样时间0.02s,v为小车的速度m/s,w为误差,一般假设该误差符合正态分布N(0, Q),0为均值,Q为方差
另一个预测值我们直接从GPS读出:
s2(k)=sg + z
式中,sg为GPS读出的值,z为GPS的噪声引起的误差,一般假设该误差符合正态分布N(0, R),0为均值,R为方差
注意:由于s1的累计误差问题,实际上,我们递推s1(k)时,并不是用的直接用的s1(k-1),而是用卡尔曼最优的估计值。
卡尔曼滤波器公式:
sk(k)= sk(k-1)+Bv + k*(sg - sk(k-1))
式中,sk(k)为第k次卡尔曼的估计值,sg为GPS读出的值,k为系数,显然这里k=1
前两项称为:预估器(在读GPS之前的预估值),第三相称为:更新值
在了解卡尔曼滤波器之前,先得来了解一下“状态观测器”。场景是这样的:在t时刻给火箭提供的燃料量为w(t),火箭喷嘴的温度为Tin,为了了解Tin(t)与w(t)的关系,我们必须要获取很多组样本(w1,Tin1)、(w2,Tin2)······,可惜喷嘴的温度太高,我们无法测量喷嘴的温度,取而代之,我们只能测量喷嘴外部的外壳的温度Text
上图(左)中,上面为真正的火箭,下面为它的数学模型,因为没有直接馈通,该模型不含D矩阵,当然,这个数学模型我们无法使它精确的描述火箭,暂且用一个一阶系统来描述,即右图。我们的目的是,通过输入量u,实时预测喷嘴真正的温度x,我们能拿到的数字量有:输入量u,实测的外壳温度y,根据这两个值,我们把u代入数学模型表达式(上面右图的下面的框图),可以得到用数学模型预测的外壳温度值和喷嘴温度,那么这个预测值能不能当做喷嘴的真实温度x呢?要回答这个问题,我们必须先研究一下这两者的差值:ex
令K=Bk,两式相减得:
解这个一阶线性微分方程,得:,其中ex0为ex在0时刻的初值
显然,当A-KC<0时,预测的喷嘴温度与真实的喷嘴温度x的差值,会随着时间的延长,逐渐变为0,这时,我们就可以用来当做x的最优预测值了。显然,增大K值,可以使e更快的变为0。
下面进入卡尔曼滤波器:
情景假设:我们有一辆小车,我们可以用遥控器任意调节小车的速度v,小车上安装了一个GPS,任务要求:让小车从一个位置沿直线出发,然后实时返回已走过的路径长度s。
小车实际的状态空间模型如上图所示,这个表达式是用离散状态空间表达式。w和v分别代表了系统误差和测量误差,一般假设两者都满足正态分布:w~N(0,Q),v~N(0, R)。Q代表了用数学模型来预测真实系统的偏离的程度。
上图中,上半部为实际的小车,下半部为我们提出的数学预测模型,这样就跟前文的火箭类似了,这里我们能通过模型给出一个预测的位置值,也能通过GPS获得一个测量值y。
真实的小车模型中带有系统误差,例如车轮打滑,或者车轮进坑等,使得路程不完全等于速度的积分。这里相比火箭问题更糟糕的情况是,测量值也有很大误差,火箭问题中,我们就把测量的外壳温度当做外壳的真实温度了,但在这里,GPS的测量路程,我们还不能完全当做真实的路程,它也有误差.
数学模型给出的预测值为:
式中,为预测值,u为输入值,yk为GPS的测量值,k是反馈系数。这个式子就是我们的状态观测器。
上式的前两项称为预估器,第三项称为更新值,把上式简化一下,变为:
实际上,我们在计算预估值,和更新值时,还得顺便计算一下新的系统误差的协方差和测量误差的协方差,也即: