Matlab中kalman函数说明

5.4.2 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(即G （n ）)。

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 Discrete time 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)
^^
^
^
^
[|][|1]([][|1])[1|][|][]
v x n n x n n M y n C x n n x n n A x n n Bu n =-+--+=+(5.11)
图5.24 Kalman 滤波器
(5.12)
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. of samples ′), 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 滤波器的设计, 我们也可以很方便地在MA TLAB 环境中完成滤波器的迭代计算。

通常的计算程序如下：
% 使用前一节产生的系统噪声 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.26 滤波前后的输出响应与真实输出的比较
图 5.25 滤波前后输出信号的比较
测量值修
正 ^
^^
1[|][|1][]([][|1]
[][|1]([][|1])[|][][|1]
T T x n n x n n M n y n C x n n M n P n n C R n CP n n C P n n I M n CP n n υ-=-+--=-+-=--预测值
^
^
[1|][|][][1|][|][]T T
x n n A x n n Bu n P n n AP n n A GQ n G +=++=+(5.17) (5.18)
定义
[]([][])[]([][])
[|]({[][|]}{[][|]})[|1]({[][|1]}{[][|1]})T T T T Q n E n n R n E n n P n n E n x n n x n x n n P n n E x n x n n x n x n n ωωυυ===---=----
图 5.27 时变滤波器的设计结果分析。