《第四章 6节卡尔曼滤波》由会员分享，可在线阅读，更多相关《第四章 6节卡尔曼滤波（10页珍藏版）》请在金锄头文库上搜索。
1、4.5 卡尔曼滤波1960 年由 Kalman 和 Bucy 提出(空间技术的发展)1) 是线性最小均方误差滤波2) 把对信号的先验知识用信号的模型形式表示3) 时域状态变量法4) 递推形式的线性最小均方误差算法。 卡尔曼滤波建立在已知随机信号模型的基础上 ；它适用于时变非平稳随机 序列。动态估计4.5.1 卡尔曼滤波的基本概念一个实际的系统可用如下形式表示：设向量非平稳序列 x 和 y 用下面的动态方程描述k-1k-1x 二 x + wJ kk ,k -1 k-1k-1(k 0)(433)y 二 C x + vkk k kx是状态向量，y是观测向量，w是输入噪声，v是观测噪声， 是 kkkk
2、k,k -1从k -1时刻到k时刻的状态转移阵。上述动态方程可由系统的机理推导得来或由 实验数据辨识得到。现假设已知。有如下假设：1) w和v为零均值白噪声即:kkkjEw = 0, Covw , w = E (w wt)= Q 8kk jk jkEv = 0, Covv , v = E(v vt) = R 8kk jk jk kj其中Q对称半正定R对称正定，均为已知。 k2)w 和 v 不相关即kkC o V w,kj(E w) = 0 弋kj,k ) j3)初始状态r是随机向量且与wk、vk不相关x (- Ex wT)=00 kx(-Ex vT)=00 k00I Covx ,w = EJ0
3、 kI Covx ,v = E0k卡尔曼滤波:状态估计在已知动态方程(433)(状态和观测方程)和样本观测数据 y ， y ，k k-1情况下，求随机序列样本状态x的估计值x。kk由方程可看出:1) 若无观测噪声可直接由y求得x ,kk2) 若无输入噪声可直接由 x 推出 x 。k-1k但实际是 w ， v 均不为零，所以不能用(433)方程求。kk卡尔曼通过对称为一步预测观测误差新息的修正加之最小均方误差调 整准则很好地解决了带有噪声的状态估计问题。现代控制理论中的状态观测器是一种最简单的估计算法。它只适用于定常系 统。卡尔曼也适用于时变系统，而且由于充分利用了噪声的统计特性，精度也高 很多
4、。卡尔曼的递推思想与新息：递推计算和新息是卡尔曼滤波的基本思想，请看 如：y + y + + yyk+1=12k+1k+1=y +(y y )kk + 1k +1k平均计算变成一种递推计算，大大减少了计算量，把k +1估计看成是在k基1础上的修正，分析修正项(y - y )它的作用：k + 1 k+1 k1) y的估计看成是在y基础上进行修正。k+1k2) y的估计可理解为对k +1次估计的预测。k3) 修正项中y - y称为一步观测误差，若一步预测误差为0说明预测很k+1k准不必修正，否则修正。14) 是对修正的加权系数，k T,修正Jk + 15) 一步预测误差y - y是第k +1的测量
5、的新信息，它提供了 y预测无法k +1kk预测的信息。称为新息(In novation)。 卡尔曼滤波器正是应用了上述新息的递推计算原则。4.5.2 卡尔曼滤波的新息递推：卡尔曼滤波对现状态X的估计左采用的是：过去状态对现状态的预测 kkF I ，加上观测偏差(y - Cf )的适当加权修正K即：k k-1k k kk-1kX = X , + K (y - C X )(434)kkk-1k k k kk-1X 是基于k -1次对k次的预测估计。k k -1左1 二 X(435)k Ik-1k,k 1 k 1所以X 二 X + K (y C X )(436)kk , k 1 k 1k k k k
6、 , k 1 k 1预测+修正=加权新息其中 K 修正加权称为增益矩阵，它决定着递推方向，质量。k如何确定K是卡尔曼滤波器的核心。把K建立成一种能够递推的结构形式 kk是卡尔曼的独到之处！实时性能好！只要增益矩阵K已知，就可以用(436)式递推地求得这一步的状态估计 k值。而K的大小反映了对观测y和新息(y -C X )的重视程度，直接影 kkk k k,k 1 k 1响到状态估计的准确性，是状态估计的关键。确定 K 的准则：最小均方误差准则。kJ 二 ex x )t (x x )L min(4一37)k kk k即J = t r 慎佥- 丘)x( - x T=t r Ph i n(4 一38
7、)KK其中tr表示方阵迹，即对角元之和。P是状态估计的均方误差矩阵。 k极小化J即439)( 440)( 441)( 442)443)dt r P 门k = 0,dKk求K。k选择K使J最小，得卡尔曼滤波器递推公式为:kP 二 PT +Qklk 1 k ,k 1 k 1 k，k1 k 1K = P CtC P Ct + R 1k k k 1 k K k k 1 k kX =O X + K (y -C X )k k , k 1 k 1 k k k k , k 1 k 1P = (I K C )P.kK K klk 1P|，K，和P的计算与观测y无关，只要条件容许，可以事先计算好,k|k 1KKK
8、而实时滤波只要递推f求状态估计即可。k卡尔曼滤波：是时域内不断地“预测修正递推”过程。1）P小说明预测较准，K也小减小修正，若在k -1时刻输入噪声w 很小Kk-1-1 -0，再由预测误差P很小由 K二P Ct C P Ct + Rk|k-1kkk-1 k L K kk-1 k /X 二 X + K （y C X ）二 Xk k ,k - 1 k - 1 k k k k k -, 1k - 1 k k - , k -1既无须考虑观测量的修正，只用状态方程即可，这符合逻辑。2） p大说明预测 X 不准,K较大,重视新息修正K （y C X ）Kk ,k -1 k -1k k k k ,k -1
9、k -1的作用，（加大实际观测与模型估计的修正作用）。3）如果在k时刻观测噪声R很小（441）kK u P| Ct k kk-1 k -再由（442）X二 Xkk , k -1 k -1C P| CtK kk-1 k -1 二 PCt (Ct )-1P-1 C-1 二 C-1kk-1 k kkk-1 kk+ C-1( y - C X ) = C-1 yk k k k , k -1 k -1k k既无须考虑状态方程，只要用观测方程直接计算即可，这也是很符合逻辑的结论 可以证明卡尔曼滤波是无偏的状态估计4.5.3 卡尔曼滤波举例举例：随机状态方程为：x + wk -1k-11丿0)x + vkk例
10、 5-1 )其中w w Ew = 0, Covw , w = E 1k 1 j kk jw w2 k 1 jw w1k 2 jw w2k 2j0 )51 kj丿例 5-2)Ev 二 0, Covv , v 二2 + (-1)k 5kk jkj这里输入噪声 w 是平稳白噪声，而观测噪声 v 是非平稳白噪声。所以即使在稳kk态时，观测 y 也是一个非平稳序列。为简单起见，初始的状态估计误差的协方k差阵设为对角阵，即( (x - x )2(x- x ) (- x)(1 0P = E1 111 22 0,(x x ) x11-x22) x (-2x 2J)010例 5-3)求增益矩阵K ,K，,K和具
11、体的卡尔曼滤波递推公式。1 2 10解 状态方程中的各参数矩阵是定常的，即1）f1 由式(例5-1) =O =k,k-12）由式（例 5-2）噪声参数为：1丿f0Qk = Q =3）则由式（440）Pi二klk-1k ,k -1预测误差协方差阵的递推公式为：f 1P =OP T + Q = 1kk-1k -1I 011f1 01f0 01P+1丿k-1J 1丿0 1丿k ,k-1Ct + RkPTk-1+ Qk-1k例 5-4 )4）则由式（441）K = Pk增益矩阵的递推公式为Ctklk -1 kK klk-1115）6）=P Ct C P Ct + R kk-1kk 1-1 = Pkf1
12、 10 丿(10P)k(例 5-5)1)则由式（443）Pk估计误差协方差阵的递推公式为=(I-KkCk)Pklk-1P = (I K C )P = I K (1 0)P kKkk-1Kk例 5-6 )令k=1将（例5-3）代入（例5-4）求得P =1011丿将P代入（例5-5）求得K = 10f 20、2110 21丿将K代入（例5-6）求得P =11f 202110 2110、21131莎丿7）同理令k=2, 3, 4，5用（例5-4）（例5-5）（例5-6）递推公式可算得前5步 的结果如下表k012345勺-1f20 101|10 11丿f 8.16 6.7116.71 7.24 丿f9.05 5115 421丿f 3.6 2.212.22.7 丿f5.62 3132丿Kk(0.95 、0.45 丿(0.73 %丿(0.9 4时K即趋于稳定的周期序列，k1(0.85,0t.4为奇数K = k (0.55,0 t去为偶数根据这一特殊情况，滤波计算变得很容易，由式(442)X 二 X + K (y - C X )kk ,k 1 k 1k k k k ,k
