Main Content

卡尔曼滤波

此示例说明如何执行卡尔曼滤波。首先,使用 kalman 命令设计一个稳态滤波器。然后,对系统进行仿真以显示它如何降低测量噪声带来的误差。此示例还说明如何实现一个时变滤波器,这对于具有非平稳噪声源的系统非常有用。

稳态卡尔曼滤波器

假设有以下输入端为高斯噪声 w、输出端为测量噪声 v 的离散被控对象:

x[n+1]=Ax[n]+Bu[n]+Gw[n]y[n]=Cx[n]+Du[n]+Hw[n]+v[n]

目标是设计一个卡尔曼滤波器,根据噪声测量值 y[n] 来估计真实的被控对象输出 yt[n]=y[n]-v[n]。这种稳态卡尔曼滤波器使用以下方程进行这种估计。

时间更新:

xˆ[n+1|n]=Axˆ[n|n-1]+Bu[n]+Gw[n]

测量更新:

xˆ[n|n]=xˆ[n|n-1]+Mx(y[n]-Cxˆ[n|n-1]-Du[n])yˆ[n|n]=Cxˆ[n|n-1]+Du[n]+My(y[n]-Cxˆ[n|n-1]-Du[n])

其中,

  • xˆ[n|n-1] 基于给定的到 y[n-1] 为止的过往测量值计算的 x[n] 的估计值。

  • xˆ[n|n]yˆ[n|n] 是估计的状态值和测量值,它们基于上一次测量值 y[n] 进行更新。

  • MxMy 是最优新信息增益,在给定噪声协方差 E(w[n]w[n]T)=Q E(v[n]v[n]T)=RN=E(w[n]v[n]T)=0 的情况下,通过最小化估计误差的稳态协方差来选择最优值。(有关如何选择这些增益的详细信息,请参阅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;

对于此示例,设置 G=B,意味着过程噪声 w 是加性输入噪声。此外,设置 H=0,意味着输入噪声 w 对输出 y 没有直接影响。这些假设产生更简单的被控对象模型:

x[n+1]=Ax[n]+Bu[n]+Bw[n]y[n]=Cx[n]+v[n]

H = 0 时,可以证明 My=CMx(请参阅kalman)。总的来说,这些假设也简化了卡尔曼滤波器的更新方程。

时间更新:

xˆ[n+1|n]=Axˆ[n|n-1]+Bu[n]+Bw[n]

测量更新:

xˆ[n|n]=xˆ[n|n-1]+Mx(y[n]-Cxˆ[n|n-1])yˆ[n|n]=Cxˆ[n|n]

要设计此滤波器,首先创建具有 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 和含噪被控对象输出 ykalmf 的第一个输出是真实被控对象输出的估计值 yˆ,其余输出是状态估计值 xˆ

kalmdemo2.png

对于此示例,丢弃状态估计值,只保留第一个输出 yˆ

kalmf = kalmf(1,:);

使用滤波器

要了解此滤波器的工作原理,请生成一些数据,并将滤波后的响应与真实的被控对象响应进行比较。完整的系统如下图所示。

kalmdemo1.png

要仿真此系统,请使用 sumblk 为测量噪声 v 创建一个输入。然后,使用 connectsys 和卡尔曼滤波器联接在一起,使得 u 是共享输入,并且含噪被控对象输出 y 馈入另一个滤波器输入。其结果是一个仿真模型,它具有输入 wvu,输出 yt(真实响应)和 ye(经过滤波的响应或估计的响应 yˆ)。信号 ytye 分别是被控对象和滤波器的输出。

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);

使用设计滤波器时使用的相同噪声协方差值 QR 生成过程噪声和传感器含噪向量。

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 生成对 wvu 输入的响应。提取 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')

Figure contains 2 axes objects. Axes object 1 with title Kalman Filter Response, xlabel Number of Samples, ylabel Output contains 2 objects of type line. These objects represent True, Filtered. Axes object 2 with xlabel Number of Samples, ylabel Error contains 2 objects of type line. These objects represent 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

时变卡尔曼滤波器设计

之前的设计假设噪声协方差不随时间而改变。即使在噪声协方差不平稳时,时变卡尔曼滤波器也能很好地工作。

时变卡尔曼滤波器具有以下更新方程。在时变滤波器中,误差协方差 P[n] 和新信息增益 Mx[n] 都可以随时间而改变。您可以修改时间和测量更新方程以考虑时变,如下所示,再次采用 G=B,因此过程噪声 w 是加性输入噪声。

时间更新:

xˆ[n+1|n]=Axˆ[n|n]+Bu[n]+Bw[n]P[n+1|n]=AP[n|n]AT+BQBT

测量更新:

xˆ[n|n]=xˆ[n|n-1]+Mx[n](y[n]-Cxˆ[n|n-1])Mx[n]=P[n|n-1]CT(CP[n|n-1]CT+R[n])-1P[n|n]=(I-Mx[n]C)P[n|n-1]yˆ[n|n]=Cxˆ[n|n]

有关这些表达式的详细信息,请参阅kalman

在 Simulink® 中,您可以使用 Kalman Filter 模块(请参阅State Estimation Using Time-Varying Kalman Filter)实现时变卡尔曼滤波器。

要在 MATLAB® 中创建时变卡尔曼滤波器,请首先生成含噪被控对象响应。仿真被控对象对之前定义的输入信号 u 和过程噪声 w 的响应。然后,将测量噪声 v 与仿真的真实响应 yt 相加以获得含噪响应 y。在此示例中,含噪向量 wv 的协方差不随时间而变化。但是,您可以对非平稳噪声使用相同的过程。

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 contains 2 axes objects. Axes object 1 with title Response with Time-Varying Kalman Filter, xlabel Number of Samples, ylabel Output contains 2 objects of type line. These objects represent True, Filtered. Axes object 2 with xlabel Number of Samples, ylabel Error contains 2 objects of type line. These objects represent True - measured, True - filtered.

时变滤波器还会在估计期间估计输出协方差。由于此示例使用平稳输入噪声,输出协方差趋于稳态值。绘制输出协方差图以确认滤波器已达到稳态。

figure
plot(t,errcov) 
xlabel('Number of Samples'), ylabel('Error Covariance'),

Figure contains an axes object. The axes object with xlabel Number of Samples, ylabel Error Covariance contains an object of type line.

从协方差图可以看出,输出协方差在大约五个采样后达到稳态。从那时起,时变滤波器具有与稳态版本相同的性能。

与稳态情况一样,滤波器降低了测量噪声引起的误差。要确认这种误差减少,请计算滤波前的误差协方差(测量误差协方差)和滤波后的误差协方差(估计误差协方差)。

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

另请参阅

相关主题

外部网站