标量的卡尔曼滤波器

1、标量的卡尔曼滤波器原理及结构卡尔曼滤波与预测是一种模型化的参数估计方法。其基础是信号产生过程的自回归（AR ）模型。由白噪声激励的一阶AR 信号产生模型（可认为是理论计算模型）如图7-12（a ）所示，其数学表示为：x (k =ax (k -1 +w (k -1 （7-136）2式中w (k -1 为零均值白噪声，其方差w ；即：E w (k =0E w (k w (j =0 k j （7-137）2E w (k w (j =w k = j数据测量模型如图7-12（b ）所示，其数学表示为：y (k = cx (k + v (k （7-138）w ( k 系统参数单位延迟v (k 测量噪声(b

2、数据测量模型(a 信号产生模型图7-12 一阶AR 信号模型与观测模型2. 标量信号的卡尔曼滤波基于（7-136）和式（7-138）的模型，对x (k 进行估计的递推算法如下： x (k =a (k x (k -1 +b (k y (k （7-139）上式右边第1项表示对上次估计结果的加权，加权系数为a (k ；第2项表示对当前测量结果的加权，加权系数为b (k 。下面推演a (k 和b (k 的关系：为实现最优估计，就需要求出能够使估计值x (k 达到最小均方误差的a (k 和b (k 。定义误差为：e (k =x (k -x (k （7-140）则均方误差定义为：p (k =E e (k

3、=E x (k -x (k 2=E a (k x (k -1 +b (k y (k -x (k 2（7-141）2为求出均方误差达到最小时的a (k 和b (k ，令p (k p (k =0=0（7-142） a (k b (k 将式（7-141）代入，可得：p (k E a (k x (k -1 +b (k y (k -x (k 2=0 a (k a (k a (k 为微分变量，其余为常量，得：E 2a (k x (k -1 +b (k y (k -x (k x (k -1=0即： E a (k x (k -1 +b (k y (k -x (k x (k -1=0（7-143）同理：p (k

4、 E a (k x (k -1 +b (k y (k -x (k 2=0 b (k b (k b (k 为微分变量，其余为常量，得：E 2a (k x (k -1 +b (k y (k -x (k y (k =0即：E a (k x (k -1 +b (k y (k -x (k y (k =0（7-144）由式（7-143）得：E a (k x (k -1 x (k -1=E x (k -b (k y (k x (k -1=0（7-145）式（7-145）左边加减a (k x (k -1 x (k -1得：E a (k x (k -1 x (k -1 +a (k x (k -1 x (k -1

5、 -a (k x (k -1 x (k -1=E x (k -b (k y (k x (k -1E a (k x (k -1 -x (k -1+a (k x (k -1x (k -1=E x (k -b (k y (k x (k -1（7-146）将（7-138）：y (k = cx (k + v (k 和（7-140）：e (k =x (k -x (k 代入（7-146）：E a (k x (k -1 -x (k -1+a (k x (k -1x (k -1=E x (k -b (k y (k x (k -1得：E a (k e (k -1+a (k x (k -1x (k -1=E x (

6、k -b (k (cx (k +v (k x (k -1a (k E e (k -1 x (k -1 +x (k -1 x (k -1=E x (k 1-cb (k -b (k v (k x (k -1（7-147）对于最优估计，存在下列正交方程：E e (k x (k -1=0，E e (k -1 x (k -1=0（7-148） E v (k x (k -1=0（7-149） E e (k y (k =0（7-150）由式（7-148）和（7-149）可将式（7-147）a (k E e (k -1 x (k -1 +x (k -1 x (k -1=E x (k 1-cb (k -b (k

7、 v (k x (k -1化简为：a (k E x (k -1 x (k -1=1-cb (k E x (k x (k -1（7-151）将式（7-136）x (k =ax (k -1 +w (k -1 代入（7-151）可得：a (k E x (k -1 x (k -1=1-cb (k E ax (k -1 x (k -1 +w (k -1 x (k -1（7-152）将式（138）y (k = cx (k + v (k 代入（7-139）x (k =a (k x (k -1 +b (k y (k 得：x (k =a (k x (k -1 +b (k cx (k +b (k v (k （7-

8、153）将式（136）x (k =ax (k -1 +w (k -1 代入（7-153）：x (k =a (k x (k -1 +acb (k x (k -1 +cb (k w (k -1 +b (k v (k （7-154）即有：x (k -1 =a (k -1 x (k -2 +acb (k -1 x (k -2 +cb (k -1 w (k -2 +b (k -1 v (k -1（7-155）式（7-155）右边项均与w (k -1 不相关，故（7-155）左右两端均乘以w (k -1 ，各项与w (k -1 乘积的均值均为零，可得：E x (k -1 w (k -1=0（7-156）将

9、（7-156）代入（7-152）a (k E x (k -1 x (k -1=1-cb (k E ax (k -1 x (k -1 +w (k -1 x (k -1得：a (k E x (k -1 x (k -1=1-cb (k aE x (k -1 x (k -1（7-157）由（7-157）可得：a (k =a 1-cb (k （7-158）这就是使均方误差达到最小的a (k 和b (k 的关系。将式（7-158）代入（7-139）：x (k =a (k x (k -1 +b (k y (k 得：x (k =a 1-cb (k x (k -1 +b (k y (k x (k =a x (k

10、 -1 +b (k y (k -ac x (k -1（7-159）式（7-159）表示标量信号的卡尔曼滤波递推算法。它不仅消除了a (k ，而且具有明显的物理意义。式中右边第一项是根据先前的测量数据对x (k 的预测，第二项是修正项，它取决于新的测量值y (k 与预测值之差。b (k 称为卡尔曼增益。实现（7-159）算法的方框图如图7-13 时变增益y (y (k =ac 测量参数系统参数单位延迟图7-133. 均方误差和卡尔曼增益的推演式（7-159）所示的卡尔曼滤波算法需要每次计算b (k 。为推演b (k 可由式（7-141）p (k =E e (k =E x (k -x (k 2可得

11、均方误差为：p (k =E e (k x (k -x (k （7-160）2将式（7-139）x (k =a (k x (k -1 +b (k y (k 代入（7-160）得：p (k =E e (k a (k x (k -1 +b (k y (k -x (k （7-161）将正交方程（7-148）和式（7-150）代入（7-161）p (k =-E e (k x (k （7-162）将测量模型式（7-138）代入（7-162），并考虑正交方程E e (k y (k =0得：1E e (k v (k （7-163） c1将e (k =x (k -x (k 代入（7-163）得：p (k =E

12、(x (k -x (k v (k cp (k =用式（7-139）x (k =a (k x (k -1 +b (k y (k 的右边代替1p (k =E x (k -x (k v (k 中的x (k c1p (k =E a (k x (k -1 +b (k y (k -x (k v (k c 1p (k =E a (k x (k -1 v (k +b (k y (k v (k -x (k v (k （7-164）c式（7-164）右边三项中，考虑到E x (k -1 v (k =0和E x (k v (k =0，故只剩一项：p (k =111E b (k y (k v (k =b (k E y

13、 (k v (k =b (k v 2（7-165） c c c故有：b (k =cp (k v 2（7-166）E y (k v (k =E v (k 2=v 2y (k 为测量值，v (k 为测量噪声。下面再讨论均方误差；将式（7-159）x (k =a x (k -1 +b (k y (k -ac x (k -1代入均方误差公式，得：p(k = Ee(k 2 = E x(k - x(k 2 = Ea x(k - 1 + b(k y(k - ac x(k - 1 - x(k 2 （7-167） Ù 在利用式（7-138）和式（7-136） ，由式（7-167）可推演出 p(k =

14、Ea1 - cb(k e(k - 1 - 1 - cb(k w(k - 1 + b(k v(k 2 （7-168） 由于 e(k-1、w(k-1和 v(k互不相关，所以（7-168）中的交叉乘积项的均值 为零，故有： 2 p(k = a 2 1 - cb(k 2 p(k - 1 - 1 - cb(k 2 s w + b 2 (k s v2 （7-169） 2 式中： p(k - 1 = Ee(k - 12 s w = Ew(k - 12 s v2 = Ev(k 2 。 1 将（7-165） p(k = b(k s v2 代入（7-169）得： c b(k s v2 2 = a 2 1 - cb

15、(k 2 p(k - 1 - 1 - cb(k 2 s w + b 2 (k s v2 c 2 2 b(k s v2 + c 2 a 2 p(k - 1 + s w = ca 2 p(k - 1 + s w （7-170） 2 ca 2 p(k - 1 + s w （7-171） 2 2 2 2 s v + c a p(k - 1 + s w 得： b(k = 注意：首先根据 p(k-1由式（7-171）计算出 b(k，再由式（7-165） p(k = 1 b(k s v2 （7-172） c 计算方差！ ！ ！ 为便于推广到向量情况，定义： 2 （7-173） p1 (k = a 2 p(k - 1 + s w 2 ca 2 p(k - 1 + s w 代入（7-171） b(k = 2 得： 2 2 2 s v + c a p(k - 1 + s w b( k = cp1 (k （7-174） s + c 2 p1 (k 2 v 将（7-174）中的 sv2 表达式代入（7-172）得： p(k = p1 (k 1 - cb(k （7-175） 卡尔曼滤波器的 4 个方程 2 p1 (k =