比如我们的观查值是Z,估计值是X, 那么新的估计值就应该是 Xnew = X + K ( Z-X),从这个公式可以看到,如果X估计小了,那么新的估计值会加上一个量K ( Z-X), 如果估计值大了,大过Z了,那么新的估计值就会减去一个量K ( Z-X),这就保证新的估计值一定比现在的准确,一次一次递归下去就会越来越准却了,当然这里面很有作用的也是这个K,也就是我们前面说的权值,书上都把他叫卡尔曼增益。。。(Xnew = X + K ( Z-X) = X ×(1-K) + KZ ,也就是说估计值X的权值是1-k,而观察值Z的权值是k,究竟k 取多大,全看估计值和观察值以前的表现,也就是他们的方差情况了)
建模:
z_t = H*x_t + v_t; (1)
x_t = A*x_(t-1) + B*u_(t-1) + w_(t-1); (2)
x&_t = x&-_t + K( z_t - H*x_(t-1) ) ; (3)
提一下,这里" & "表示估计值," - "表示是用前面式子算出来的估计值,不带" - "表示是最后的估计值。 (3)这个式子是想说,你不是式子估计和观测值都不准么,那么你把他俩加个权,求个和,那就可能更准确了。啥?你说这个式子不像加权?你把丫拆了,倒腾倒腾不就像了。 所以,最牛B就牛B在了这个“K”,传说中的卡尔曼增益。 这个K怎么得到的?我也不知道。 文章说法是,定义误差 e_t = x_t - x&_t ,P_t为此误差的协方差矩阵,目的是使这个误差协方差矩阵最小化,把(3)代过去,于是折腾来折腾去,再求个导数为0,解得K,这个关键值的算法:
K = P-_t * H^T * ( H * P-_t * H^T + R) ^(-1); (4)
哦,对了,另外注意一点,从此以后,我们就都在估计上做文章了,你只要记得,咱永远看不到真实值就行了,于是我们的式子里不是带"&"就是带"-"。
那么式子(4)就是在说,你丫这么着就把K求出来了。于是,问题就变成了这个P-_t怎么个求法。
说到这里,传说中的卡尔曼滤波就算讲完了。神马?你说P-_k还没求呢。是啊,卡尔曼滤波一共就俩需要求的玩意儿,还都tm是迭代求解,一个就是这个P-_t,另一个就是状态x-_t。你随便给个初始值,然后就迭着吧。
言归正传,我还得给出迭代的公式:
x-_t = A * x&_(t-1) + B * u_(t-1); (5)
P-_t = A * P_(t-1) * A^T + Q; (6)
大家一定别搞混Q和R谁是哪个公式冒出来的啊。 另外严重关切一下这里"-","&"以及不加的关系。 注意到啥没有?对了,(6)式中等号右边的P_(t-1)不带任何符号,嘿嘿,那自然是还差一个公式啦:
P_t = (I - K_t * H ) P-_(t-1); (7)
大功告成,以上就是传说中的卡尔曼滤波的迭代求解方法,如果你要预测状态呢,就用式子(5)的结果;如果预测观测值呢,就把式子(5)的结果乘个H;如果是监测状态呢,就用式子(3)的结果。