4-卡尔曼滤波(2)
4
卡尔曼滤波器的设计与分析
设模型是一个标量系统
4.1 系统模型
⎧ xk +1 = xk ⎨ ⎩ z k = xk + vk
式中, v k 设 P0
~ (0, r ), x0 ~ (m0 , P0 )。
= 1、 r = 10 , x0 = 3,设计 Kalman 滤波器。
4.2 算法程序设计
clear; randn('seed',0); %初始值 x0=3; xg0=0; P0=1; R=10; Q=0;
%初始状态 %初始状态估值 %初始方差估值 %测量噪声方差 %过程噪声方差
%系统模型参数赋值 A=1; H=1; G=0; %系统矩阵 %测量矩阵 %过程噪声驱动矩阵 %测量时刻 %状态真实值 %状态估计值 %滤波器增益 %误差方差 %测量值算数平均值
JG=zeros(1,6); JG(1,1)=0; JG(1,2)=x0; JG(1,3)=0; JG(1,4)=0; JG(1,5)=P0; JG(1,6)=0;
num=0; temp=0;
%滤波器递推算法 for i=1:1999 zk=x0+randn*sqrt(R); temp=temp+zk; %一步预测 xg1=A*xg0; P1=A*P0*A'+G*Q*G'; %测量修正 K=P1*H'*inv(H*P1*H'+R); xg0=xg1+K*(zk-H*xg1); P0=(1-K*H)*P1; %真实测量值
%存储滤波结果 num=num+1; JG(i+1,1)=num; JG(i+1,2)=x0; JG(i+1,3)=xg0; JG(i+1,4)=K; JG(i+1,5)=P0; end %测量时刻 %状态真实值 %状态估计值 %滤波器增益 %误差方差 %测量值算数平均值
JG(i+1,6)=temp/i;
4.3 仿真结果
当k
→ ∞ 时,滤波结果收敛于算术平均值。
当k
→ ∞ 时, K ( k ) → 0 。
当k
→ ∞ 时, P ( k ) → 0 。
4.4 初始参数取值对滤波器性能的影响
(1)方差阵初始值的影响
P0 = 1
P0 = 10
P0 = 1
P0 = 10
P0 = 1
P0 = 10
P0 = 1:
时刻 0 1.0000 2.0000 3.0000 4.0000
滤波值 0 0.6076 0.9722 1.1464 1.3582
增益 0 0.0909 0.0833 0.0769 0.0714
方差 1.0000 0.9091 0.8333 0.7692 0.7143
算术平均值 0 6.6839 5.8331 4.9679 4.7539
P0 = 10 :
0 1.0000 2.0000 3.0000 4.0000 0 3.3420 3.8887 3.7259 3.8031 0 0.5000 0.3333 0.2500 0.2000 10.0000 5.0000 3.3333 2.5000 2.0000 0 6.6839 5.8331 4.9679 4.7539
(2)状态初始估计值的影响( P 0 时刻 0 1.0000 2.0000 3.0000 4.0000 0 1.0000 2.0000 3.0000 4.0000 滤波值 0 3.3420 3.8887 3.7259 3.8031 增益 0 0.5000 0.3333 0.2500 0.2000 0
= 10 )
方差 算术平均值 10.0000 0 5.0000 6.6839 3.3333 5.8331 2.5000 4.9679 2.0000 4.7539 10.0000 5.0000 3.3333 2.5000 2.0000 0 6.6839 5.8331 4.9679 4.7539
-3.0000 1.8420 2.8887 2.9759 3.2031
0.5000 0.3333 0.2500 0.2000
4.5 影响 Kalman 滤波器性能的因素分析
【例 1】估计一个常值标量 x ,测量有白噪声,系统方程和测量方程如 下:
x(k + 1) = x(k ) z (k + 1) = x(k + 1) + v(k + 1)
式中, x(k )、 z (k
+ 1) ∈ R ; v(k + 1)∼ N (0, r ), x(0 )∼ N (m, P0 )。
求 x 的滤波结果,并分析测量噪声 r 的取值对滤波结果的影响。
解:状态一步预测: x ˆ (k 预测方差: 增益矩阵:
+1 k ) = x ˆ (k )
P (k + 1 k ) = P (k )
P (k + 1 k ) K (k + 1) = P (k + 1 k ) + r
状态最优估计:
x ˆ (k + 1) = x ˆ (k + 1 k ) + K (k + 1)[z (k + 1) − x ˆ (k + 1 k )]
修正方差:
rP(k + 1 k ) P(k + 1) = P (k + 1 k ) + r
x ˆ 0 、 P0 给定,有如下递
推结果:
当k
= k时
当k
k +1 i =1
→ ∞时
x ˆ (k + 1) =
mr + P0 ∑ z (i ) r + (k + 1)P0
1 k +1 x ˆ (k + 1) = ∑ z (i ) k + 1 i =1
rP0 P(k + 1) = r + (k + 1)P0 P0 K (k + 1) = r + (k + 1)P0
P(k + 1) = 0 K (k + 1) = 0
结论:
(1)对常值的估值,随着测量次数增加,最后趋于算术平均值,而 其估值方差等于零。 (2)设 r
→ ∞ ,即测量噪声大到无法测量,则有
x ˆ (k + 1) = m = x0 , P(k + 1) = P0
(3)设 x0
= m = 0 , P0 → ∞ ,即对未知量缺少先验知识,那么
x ˆ (k + 1)和 P(k + 1)给出最大似然估计
P0 1 K (k + 1) = = (k + 1)P0 + r k + 1 x ˆ (k + 1) = x ˆ (k ) + K (k + 1)[z (k + 1) − x ˆ (k )] 1 [z (k + 1) − x =x ˆ (k ) + ˆ (k )] k +1
恰好是一个递推采样平均方程(当 r ,且 x ˆ (0) = 0
= 0 时,正好是上述同样结论) 。
【例 2】考虑一个标量过程
x(k + 1) = ax(k ) + w(k ) z (k ) = x (k ) + v (k )
设{w(k )}是一个 N (0, q ),{v(k )}是一个 N (0, r ), x(0)是 N (0, P 0 ),
a 是一个常值,两个高斯白噪声序列和 x(0)是相互独立的。试分析影响
Kalman Filter 估计性能的因素。
解:
采用 Kalman Filter 的递推计算公式为:
x ˆ (k k − 1) = ax ˆ (k − 1) P(k k − 1) = a 2 P(k − 1) + q
K (k ) = a P(k − 1) + q ⋅ a P(k − 1) + q + r
2 2
[
][
]
−1
x ˆ (k ) = x ˆ (k k − 1) + K (k )[z (k ) − x ˆ (k k − 1)]
P(k ) = r a P(k − 1) + q ⋅ a P(k − 1) + q + r
2 2
[
][
]
−1
初始条件: x ˆ (− 1) = 0, P(− 1) = P0
分析结论: 1. 因为 P(k
− 1) ≥ 0(定义) ,可以有 P(k k − 1) ≥ q ,说明系统 本身干扰的大小 q 限制了预测精度。 K (k ) ≤ 1时,从误差的增益方程有 P(k ) = rK (k ),所以,限定了 0 ≤ P(k ) ≤ r ,对于所有的 k ≥ 0。
2. 在大多数情况下,当 0 ≤
3. 当 q
>> r 时,有 K (k ) = 1和 P(k ) = r ,说明滤波器精度的限
制由测量误差的方差决定。 4. 当 P 0 很大时 (>> 少到 P (1) ≤
r ),从第一个测量就将滤波器误差方差从 P0 减
r