Kalman滤波器的设计
这一节将讨论如何使用控制系统工具箱进行Kalman滤波器的设计和仿真。 考虑下面的离散系统：
x［n+1］=Ax［n］+B(u［n］+w［n］) (5.9)
y［n］=Cx［n］ (5.10)
其中, w［n］是在输入端加入的高斯噪声。 状态矩阵参数分别为
A = ［1.1269-0.49400.1129
1.0000 0 0
0 1.0000 0］;
B = ［-0.3832
0.5919
0.5191］;
C = ［1 0 0］;
我们的目标是设计Kalman滤波器, 在给定输入u［n］和带噪输出测量值
yv［n］=Cx［n］+v［n］的情况下估计系统的输出。 其中, v［n］是高斯白噪声。
1） 离散Kalman滤波器
上述问题的稳态Kalman滤波器方程如下：
测量值修正计算
2）稳态设计
我们可以通过kalman函数设计上述稳态滤波器。 首先定义带噪声的系统模型：
x［n+1］=Ax［n］+Bu［n］+Bw［n］ (状态方程)
y［n］=Cx［n］ (测量方程)
具体的程序代码如下：
% 注意:设置采样时间为-1表示模型为离散的
Plant = ss (A, ［B B］, C, 0, -1, ′inputname′, {′u′ ′w′}, ′outputname′, ′y′);
假设Q=R=1, 下面可以设计离散Kalman滤波器：
Q = 1; R = 1;
［kalmf, L,P, M］ = kalman(Plant, Q, R);
函数将返回Kalman滤波器的状态模型kalmf和修正增益M。
M =[
3.7980e-01
8.1732e-02
-2.5704e-01]
因为我们对输出估计ye比较感兴趣, 因此只需要保留kalmf的第一个输出。 为此输入
kalmf = kalmf(1, :);
kalmf
a =
x1 -e x2 -e x3 -e
x1 -e 0.7683 -0.494 0.1129
x2 -e 0.62020 0
x3 -e -0.0817321 0
b =
u y
x1 -e -0.3832 0.3586
x2 -e 0.5919 0.3798
x3 -e 0.5191 0.081732
c =
x1-e x2-e x3-e
y -e 0.6202 0 0
d =
u y
y-e 0 0.3798
I/O groups:
Group name I/O Channel(s)
KnownInput I 1
Measurement I 2
OutputEstimate O 1
Sampling time:unspecified
Discretetime model.
滤波器的功能是在已知输入噪声方差的条件下尽可能消除输出信号中的噪声影响。 图5.25显示了滤波前后的不同输出信号。
下面用程序来比较滤波后输出信号与系统实际信号相对理想输出的误差。
a = A;
b = ［B B 0*B］;
c = ［C;C］;
d = ［0 0 0;0 0 1］;
P = ss(a,b,c,d,-1, ′inputname′, {′u′ ′w′ ′v′},′outputname′, {′y′′yv′});
sys = parallel(P,kalmf, 1, 1, ［ ］, ［ ］) % 创建并联系统
% 将系统输出yv正反馈到滤波器的输入端, 形成闭环系统
SimModel =feedback(sys, 1, 4, 2, 1)
% 从I/O列表中删除yv
SimModel =SimModel(［1 3］, ［1 2 3］)
% 产生高斯噪声信号
t = ［0:100］′;
u = sin(t/5);
n = length(t)
randn(′seed′, 0)
w =sqrt(Q)*randn(n, 1);
v =sqrt(R)*randn(n, 1);
% 系统仿真
［out, x］ =lsim(SimModel, ［w, v, u］);
y = out(:, 1); % 系统真实（理想）输出响应
ye = out(:, 2);% 滤波后的系统输出
yv = y + v;% 系统输出的测量值
% 比较结果
subplot(211), plot(t, y, ′--′, t, ye, ′-′),
xlabel(′No. of samples′), ylabel(′Output′)
title(′Kalman filter response′)
subplot(212),plot(t, y-yv, ′-.′, t, y-ye, ′-′), xlabel(′No. ofsamples′), ylabel(′Error′)
比较结果如图5.26所示。图中上面的图形显示的是真实响应y（虚线）和滤波后的输出ye（实线）， 下面的图形比较测量误差（虚线）与估计误差（实线）。 该图表明, 滤波器最大程度地消除了系统输出中的噪声影响。 这可以通过计算误差的协方差进行验证。
MeasErr = y-yv;
MeasErrCov =sum(MeasErr.*MeasErr)/length(MeasErr);
EstErr = y-ye;
EstErrCov =sum(EstErr.*EstErr)/length(EstErr);
滤波前误差（测量误差）的协方差为
MeasErrCov
MeasErrCov =
1.1138
而滤波后的误差（估计误差）的协方差仅为
EstErrCov
EstErrCov =
0.2722
3）时变Kalman滤波器
时变Kalman滤波器是稳态滤波器在时变系统或具有可变协方差噪声的LTI系统的推广。 假定系统状态方程和测量方程分别为
x［n+1］=Ax［n］+Bu［n］+Gw［n］
yv［n］=Cx［n］+v［n］ (5.13)
4）时变滤波器设计
尽管控制系统工具箱没有提供专门的命令完成时变Kalman滤波器的设计, 我们也可以很方便地在MATLAB环境中完成滤波器的迭代计算。 通常的计算程序如下：
% 使用前一节产生的系统噪声 w 和测量噪声 v
sys = ss(A, B, C,0, -1);
y = lsim(sys, u+w); % w = 系统噪声
yv = y + v; % v = 测量噪声
% 定义迭代的初始条件
P = B*Q*B′; % 误差协方差的初始条件矩阵
x = zeros(3,1); % 初始状态矩阵
ye =zeros(length(t), 1);
ycov =zeros(length(t), 1);
for i=1:length(t)
% 测量值修正计算
Mn = P*C′/(C*P*C′+R);
x = x + Mn*(yv(i)-C*x); % x［n|n］
P = (eye(3)-Mn*C)*P; % P［n|n］
ye(i) = C*x;
errcov(i) = C*P*C′;
% 下一时刻的预测值
x= A*x + B*u(i); % x［n+1|n］
P = A*P*A′ + B*Q*B′; % P［n+1|n］
end
subplot(211), plot(t, y,′--′, t, ye, ′-′) % 显示相关的滤波结果
title(′Time-varying Kalman filter response′)
xlabel(′No. of samples′), ylabel(′Output′)
subplot(212), plot(t, y-yv, ′-.′, t, y-ye, ′-′)
xlabel(′No. of samples′), ylabel(′Output′)
图 5.27 时变滤波器的设计结果分析
%卡尔曼滤波
clear
N=800;
w(1)=0;
w=randn(1,N) %系统预测的随机白噪声
x(1)=0;
a=1;
for k=2:N;
x(k)=a*x(k-1)+w(k-1);%系统的预测值
end
V=randn(1,N); %测量值的随机白噪声
q1=std(V);
Rvv=q1.^2;
q2=std(x);
Rxx=q2.^2;
q3=std(w);
Rww=q3.^2;
c=0.2;
Y=c*x+V; %测量值
p(1)=0;
s(1)=0;
for t=2:N;
p1(t)=a.^2*p(t-1)+Rww;%前一时刻X的相关系数
b(t)=c*p1(t)/(c.^2*p1(t)+Rvv);%卡尔曼增益
s(t)=a*s(t-1)+b(t)*(Y(t)-a*c*s(t-1));%经过滤波后的信号
p(t)=p1(t)-c*b(t)*p1(t);%t状态下x(t|t)的相关系数
end
figure(1)
plot(x)
title('系统的预测值')
figure(2)
plot(Y)
title('测量值')
figure(3)
plot(s)
title('滤波后的信号')