卡尔曼滤波
此示例说明如何执行卡尔曼滤波。首先,使用 kalman
命令设计一个稳态滤波器。然后,对系统进行仿真以显示它如何降低测量噪声带来的误差。此示例还说明如何实现一个时变滤波器,这对于具有非平稳噪声源的系统非常有用。
稳态卡尔曼滤波器
假设有以下输入端为高斯噪声 w、输出端为测量噪声 v 的离散被控对象:
目标是设计一个卡尔曼滤波器,根据噪声测量值 来估计真实的被控对象输出 。这种稳态卡尔曼滤波器使用以下方程进行这种估计。
时间更新:
测量更新:
其中,
基于给定的到 为止的过往测量值计算的 的估计值。
和 是估计的状态值和测量值,它们基于上一次测量值 进行更新。
和 是最优新信息增益,在给定噪声协方差 、 和 的情况下,通过最小化估计误差的稳态协方差来选择最优值。(有关如何选择这些增益的详细信息,请参阅
kalman
。)
(这些更新方程描述一个 current
类型估计器。有关 current
估计器和 delayed
估计器之间差异的信息,请参阅kalman
。)
设计滤波器
您可以使用 kalman
函数来设计此稳态卡尔曼滤波器。此函数根据您提供的过程噪声协方差 Q 和传感器噪声协方差 R 确定特定被控对象的最佳稳态滤波器增益 M。对于此示例,使用以下值作为被控对象的状态空间矩阵。
A = [1.1269 -0.4940 0.1129 1.0000 0 0 0 1.0000 0]; B = [-0.3832 0.5919 0.5191]; C = [1 0 0]; D = 0;
对于此示例,设置 ,意味着过程噪声 w 是加性输入噪声。此外,设置 ,意味着输入噪声 w 对输出 y 没有直接影响。这些假设产生更简单的被控对象模型:
当 H = 0 时,可以证明 (请参阅kalman
)。总的来说,这些假设也简化了卡尔曼滤波器的更新方程。
时间更新:
测量更新:
要设计此滤波器,首先创建具有 w
的一个输入的被控对象模型。将采样时间设置为 -1
以将被控对象标记为离散(没有特定采样时间)。
Ts = -1; sys = ss(A,[B B],C,D,Ts,'InputName',{'u' 'w'},'OutputName','y'); % Plant dynamics and additive input noise w
过程噪声协方差 Q
和传感器噪声协方差 R
是大于零的值,通常从对系统的研究或测量值中获得。对于此示例,请指定以下值。
Q = 2.3; R = 1;
使用 kalman
命令设计滤波器。
[kalmf,L,~,Mx,Z] = kalman(sys,Q,R);
此命令设计卡尔曼滤波器 kalmf
,这是实现时间更新和测量更新方程的状态空间模型。滤波器输入是被控对象输入 u 和含噪被控对象输出 y。kalmf
的第一个输出是真实被控对象输出的估计值 ,其余输出是状态估计值 。
对于此示例,丢弃状态估计值,只保留第一个输出 。
kalmf = kalmf(1,:);
使用滤波器
要了解此滤波器的工作原理,请生成一些数据,并将滤波后的响应与真实的被控对象响应进行比较。完整的系统如下图所示。
要仿真此系统,请使用 sumblk
为测量噪声 v
创建一个输入。然后,使用 connect
将 sys
和卡尔曼滤波器联接在一起,使得 u
是共享输入,并且含噪被控对象输出 y
馈入另一个滤波器输入。其结果是一个仿真模型,它具有输入 w
、v
和 u
,输出 yt
(真实响应)和 ye
(经过滤波的响应或估计的响应 )。信号 yt
和 ye
分别是被控对象和滤波器的输出。
sys.InputName = {'u','w'}; sys.OutputName = {'yt'}; vIn = sumblk('y=yt+v'); kalmf.InputName = {'u','y'}; kalmf.OutputName = 'ye'; SimModel = connect(sys,vIn,kalmf,{'u','w','v'},{'yt','ye'});
要模拟滤波器行为,需要生成一个已知正弦输入向量。
t = (0:100)'; u = sin(t/5);
使用设计滤波器时使用的相同噪声协方差值 Q
和 R
生成过程噪声和传感器含噪向量。
rng(10,'twister');
w = sqrt(Q)*randn(length(t),1);
v = sqrt(R)*randn(length(t),1);
最后,使用 lsim
仿真响应。
out = lsim(SimModel,[u,w,v]);
lsim
在输出端 yt
和 ye 生成对 w
、v
和 u
输入的响应。提取 yt
和 ye 通道,并计算测量的响应。
yt = out(:,1); % true response ye = out(:,2); % filtered response y = yt + v; % measured response
将真实响应与滤波后的响应进行比较。
clf subplot(211), plot(t,yt,'b',t,ye,'r--'), xlabel('Number of Samples'), ylabel('Output') title('Kalman Filter Response') legend('True','Filtered') subplot(212), plot(t,yt-y,'g',t,yt-ye,'r--'), xlabel('Number of Samples'), ylabel('Error') legend('True - measured','True - filtered')
如第二个图所示,卡尔曼滤波器能够减少由于测量噪声引起的误差 yt - y
。要确认这种误差减少,请计算滤波前的误差协方差(测量误差协方差)和滤波后的误差协方差(估计误差协方差)。
MeasErr = yt - y; MeasErrCov = sum(MeasErr.*MeasErr)/length(MeasErr)
MeasErrCov = 0.9871
EstErr = yt - ye; EstErrCov = sum(EstErr.*EstErr)/length(EstErr)
EstErrCov = 0.3479
时变卡尔曼滤波器设计
之前的设计假设噪声协方差不随时间而改变。即使在噪声协方差不平稳时,时变卡尔曼滤波器也能很好地工作。
时变卡尔曼滤波器具有以下更新方程。在时变滤波器中,误差协方差 和新信息增益 都可以随时间而改变。您可以修改时间和测量更新方程以考虑时变,如下所示,再次采用 ,因此过程噪声 w 是加性输入噪声。
时间更新:
测量更新:
有关这些表达式的详细信息,请参阅kalman
。
在 Simulink® 中,您可以使用 Kalman Filter 模块(请参阅State Estimation Using Time-Varying Kalman Filter)实现时变卡尔曼滤波器。
要在 MATLAB® 中创建时变卡尔曼滤波器,请首先生成含噪被控对象响应。仿真被控对象对之前定义的输入信号 u
和过程噪声 w
的响应。然后,将测量噪声 v
与仿真的真实响应 yt
相加以获得含噪响应 y
。在此示例中,含噪向量 w
和 v
的协方差不随时间而变化。但是,您可以对非平稳噪声使用相同的过程。
yt = lsim(sys,[u w]); y = yt + v;
接下来,在 for
循环中实现递归滤波器更新方程。
P = B*Q*B'; % Initial error covariance x = zeros(3,1); % Initial condition on the state ye = zeros(length(t),1); ycov = zeros(length(t),1); errcov = zeros(length(t),1); for i=1:length(t) % Measurement update Mxn = P*C'/(C*P*C'+R); x = x + Mxn*(y(i)-C*x); % x[n|n] P = (eye(3)-Mxn*C)*P; % P[n|n] ye(i) = C*x; errcov(i) = C*P*C'; % Time update 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,yt,'b',t,ye,'r--') xlabel('Number of Samples'), ylabel('Output') title('Response with Time-Varying Kalman Filter') legend('True','Filtered') subplot(212), plot(t,yt-y,'g',t,yt-ye,'r--'), xlabel('Number of Samples'), ylabel('Error') legend('True - measured','True - filtered')
时变滤波器还会在估计期间估计输出协方差。由于此示例使用平稳输入噪声,输出协方差趋于稳态值。绘制输出协方差图以确认滤波器已达到稳态。
figure plot(t,errcov) xlabel('Number of Samples'), ylabel('Error Covariance'),
从协方差图可以看出,输出协方差在大约五个采样后达到稳态。从那时起,时变滤波器具有与稳态版本相同的性能。
与稳态情况一样,滤波器降低了测量噪声引起的误差。要确认这种误差减少,请计算滤波前的误差协方差(测量误差协方差)和滤波后的误差协方差(估计误差协方差)。
MeasErr = yt - y; MeasErrCov = sum(MeasErr.*MeasErr)/length(MeasErr)
MeasErrCov = 0.9871
EstErr = yt - ye; EstErrCov = sum(EstErr.*EstErr)/length(EstErr)
EstErrCov = 0.3479
最后,当时变滤波器达到稳态时,增益矩阵 Mxn
中的值与 kalman
为稳态滤波器计算的值匹配。
Mx,Mxn
Mx = 3×1
0.5345
0.0101
-0.4776
Mxn = 3×1
0.5345
0.0101
-0.4776