卡尔曼滤波

打开实时脚本

本案例研究说明了卡尔曼滤波器的设计和仿真，同时考虑了稳态和时变卡尔曼滤波器。

植物动力学

考虑具有加性高斯噪声的离散对象 w ［ n ］ 关于输入 u ［ n ］ ：

x ［ n + 1 ］ ＝ 一个 x ［ n ］ + B （ u ［ n ］ + w ［ n ］ ） y ［ n ］ ＝ C x ［ n ］ ．

此外,让 y v ［ n ］ 对输出进行噪声测量 y ［ n ］ ，与 v ［ n ］ 表示测量噪声：

y v ［ n ］ ＝ y ［ n ］ + v ［ n ］ ．

以下矩阵表示工厂的动态。

A = [1.1269 -0.4940 0.1129;1.0000 0 0;1.0000 0 0);B = (-0.3832;0.5919;0.5191);C = [1 0 0];

离散卡尔曼滤波器

给出了该问题的稳态卡尔曼滤波方程。

  • 测量更新:

x ˆ ［ n | n ］ ＝ x ˆ ［ n | n - 1 ］ + 米 （ y v ［ n ］ - C x ˆ ［ n | n - 1 ］ ）

  • 时间更新:

x ˆ ［ n + 1 | n ］ ＝ 一个 x ˆ ［ n | n ］ + B u ［ n ］

在这些方程式中：

  • x ˆ ［ n | n - 1 ］ 为 x ［ n ］ ，鉴于过去的测量结果 y v ［ n - 1 ］ ．

  • x ˆ ［ n | n ］ 更新的估计是基于上次的测量吗 y v ［ n ］ ．

根据目前的估计 x ˆ ［ n | n ］ ，时间更新将预测下一个样本的状态值n+ 1(提前一步预测)。测量更新然后根据新的测量调整这个预测 y v ［ n + 1 ］ 校正项是创新的函数，即测量值和预测值之间的差异 y ［ n + 1 ］ ．这个差异是由以下原因造成的:

y v ［ n + 1 ］ - C x ˆ ［ n + 1 | n ］

在给定噪声协方差的情况下，选择新息增益M以最小化估计误差的稳态协方差：

E （ w ［ n ］ w ［ n ］ T ） ＝ 问 E （ v ［ n ］ v ［ n ］ T ） ＝ R N ＝ E （ w ［ n ］ v ［ n ］ T ） ＝ 0

你可以将时间和测量更新方程组合成一个状态空间模型，卡尔曼滤波器:

x ˆ ［ n + 1 | n ］ ＝ 一个 （ 我 - 米 C ） x ˆ ［ n | n - 1 ］ + ［ B 一个 米 ］ ［ u ［ n ］ y v ［ n ］ ］ y ˆ ［ n | n ］ ＝ C （ 我 - 米 C ） x ˆ ［ n | n - 1 ］ + C 米 y v ［ n ］ ．

该滤波器生成最佳估计 y ˆ ［ n | n ］ 属于 y n . 请注意，过滤器状态为 x ˆ ［ n | n - 1 ］ ．

稳态设计

您可以使用该函数设计上述稳态卡尔曼滤波器卡尔曼.首先指定带有过程噪声的工厂模型：

x ［ n + 1 ］ ＝ 一个 x ［ n ］ + B u ［ n ］ + B w ［ n ］ y ［ n ］ ＝ C x ［ n ］

这里，第一个表达式是状态方程，第二个是测量方程。

以下命令指定此工厂模型。采样时间设置为-1，以将模型标记为离散，而不指定采样时间。

(A，[B B]，C,0，-1，)“inputname”,{“u”' w '},“outputname”，“是的”);

假设问＝R= 1，设计离散卡尔曼滤波器。

Q = 1;R = 1;[kalmf L P M] =卡尔曼(植物,Q, R);

这个命令返回一个状态空间模型kalmf以及创新收益米．

米
M=3×10.3798 0.0817 -0.2570

的输入kalmf是u和 y v ，和。输出为电厂输出和状态估计， y e ＝ y ˆ ［ n | n ］ 和 x ˆ ［ n | n ］ ．

因为你对输出估计感兴趣 y e ，选择的第一个输出kalmf把剩下的扔掉。

: kalmf = kalmf (1);

为了了解滤波器是如何工作的，生成一些输入数据和随机噪声，并比较滤波后的响应 y e 有真实的反应y．您可以分别生成每个响应，也可以同时生成两个响应。要分别模拟每个响应，请使用lsim首先单独使用电厂，然后将电厂和过滤器连接在一起。接下来详细介绍联合模拟方案。

下面的框图显示了如何生成true和过滤后的输出。

您可以使用这些函数构建这个框图的状态空间模型平行的和反馈．首先建立一个完整的工厂模型u，w，v作为输入,y和 y v (测量)作为输出。

a=a；b=[BB0*b]；c=[c；c]；d=[0 0；0 0 1]；P=ss（a，b，c，d，-1，“inputname”,{“u”' w '“v”},“outputname”,{“是的”“yv”})；

然后使用平行的形成下图的并联连接。

sys=并行（P，kalmf，1,1，[]，[]）；

最后，通过连接工厂输出关闭传感器回路 y v 过滤输入 y v 有积极的反馈。

SimModel=反馈（sys，1,4,2,1）；%在输入#4和输出#2周围关闭循环SimModel = SimModel([1 3]，[1 2 3]);%从I/O列表中删除yv

所得到的仿真模型已经过验证w，v，u作为输入,y和 y e 作为输出。查看InputName和OutputName要验证的属性。

SimModel。InputName
ans=3x1电池{'w'}{'v'}{'u'}
SimModel。OutputName
ans=2 x1细胞{'y'}{'y_e'}

现在可以模拟过滤器行为了。生成一个正弦输入u处理和测量噪声向量w和v．

t =[0:100]”;u =罪(t / 5);n =长度(t);rng默认的w = sqrt (Q) * randn (n, 1);v = sqrt (R) * randn (n, 1);

模拟反应。

(, x) = lsim (SimModel [w, v, u]);y = (: 1);%真实反应你们= (:,2);%过滤响应Yv = y + v;%响应测量

比较真实的和过滤后的反应。

子地块（211），地块（t，y，'--'t,叶,，“- - -”)，xlabel(“不。样品的)，伊拉贝尔(“输出”)头衔(“卡尔曼滤波器响应”)子地块（212），地块（t，y-yv，'-.'t y-ye“- - -”)，xlabel(“不。样品的)，伊拉贝尔(“错误”）

第一个图显示了真实的反应y（虚线）和过滤后的输出 y e （实线）。第二个图比较了测量误差（虚线点）和估计误差（实线）。该图显示噪声级已显著降低。这通过计算协方差误差得到证实。滤波前的误差协方差（测量误差）为：

MeasErr = y-yv;MeasErrCov = (MeasErr。* MeasErr)之和/长度(MeasErr)
MeasErrCov = 0.9992

滤波后的误差协方差（估计误差）减小：

EstErr = y-ye;EstErrCov = (EstErr。* EstErr)之和/长度(EstErr)
埃斯特尔科夫=0.4944

时变卡尔曼滤波器

时变卡尔曼滤波器是时变系统或具有非平稳噪声协方差的LTI系统稳态滤波器的推广。

考虑下列植物状态和测量方程。

x ［ n + 1 ］ ＝ 一个 x ［ n ］ + B u ［ n ］ + G w ［ n ］ y v ［ n ］ ＝ C x ［ n ］ + v ［ n ］ ．

时变Kalman滤波器由以下递归给出：

  • 测量更新:

x ˆ ［ n | n ］ ＝ x ˆ ［ n | n - 1 ］ + 米 ［ n ］ （ y v ［ n ］ - C x ˆ ［ n | n - 1 ］ ） 米 ［ n ］ ＝ P ［ n | n - 1 ］ C T （ R ［ n ］ + C P ［ n | n - 1 ］ C T ） - 1 P ［ n | n ］ ＝ （ 我 - 米 ［ n ］ C ） P ［ n | n - 1 ］ ．

  • 时间更新:

x ˆ ［ n + 1 | n ］ ＝ 一个 x ˆ ［ n | n ］ + B u ［ n ］ P ［ n + 1 | n ］ ＝ 一个 P ［ n | n ］ 一个 T + G 问 ［ n ］ G T ．

在这里, x ˆ ［ n | n - 1 ］ 和 x ˆ ［ n | n ］ 如前所述。此外：

问 ［ n ］ ＝ E （ w ［ n ］ w ［ n ］ T ） R ［ n ］ ＝ E （ v ［ n ］ v ［ n ］ T ） P ［ n | n ］ ＝ E （ ｛ x ［ n ］ - x ［ n | n ］ ｝ ｛ x ［ n ］ - x ［ n | n ］ ｝ T ） P ［ n | n - 1 ］ ＝ E （ ｛ x ［ n ］ - x ［ n | n - 1 ］ ｝ ｛ x ［ n ］ - x ［ n | n - 1 ］ ｝ T ） ．

为简单起见，删除了表示状态空间矩阵的时间依赖性的下标。

给定的初始条件 x ［ 1 | 0 ］ 和 P ［ 1 | 0 ］ ，您可以迭代这些方程来执行过滤。您必须更新两个状态估计 x ［ n | ． ］ 误差协方差矩阵 P ［ n | ． ］ 每次取样。

时变设计

为了实现这些滤波器递归，首先生成噪声输出测量值。使用过程噪声w和测量噪声v前面生成的。

sys = ss (A, B, C, 0,1);y = lsim (sys, u + w);Yv = y + v;

假设初始条件如下:

x ［ 1 | 0 ］ ＝ 0 ， P ［ 1 | 0 ］ ＝ B 问 B T

使用为循环。

P = B * * B”;%初始误差协方差x=零（3,1）；%状态的初始条件ye=零（长度（t），1）；ycov=零（长度（t），1）；为i = 1:长度(t)%测量更新Mn=P*C'/（C*P*C'+R）；x=x+Mn*（yv（i）-C*x）；% x [n | n]P =(眼(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]结束

比较真实的和估计的输出图形。

子地块（211），地块（t，y，'--'t,叶,，“- - -”)头衔(“时变卡尔曼滤波器响应”)xlabel(“不。样品的)，伊拉贝尔(“输出”)子地块（212），地块（t，y-yv，'-.'t y-ye“- - -”)xlabel(“不。样品的)，伊拉贝尔(“输出”）

第一个图显示了真实的反应y(虚线)和过滤后的响应 y e （实线）。第二个图比较测量误差（虚线点）和估计误差（实线）。

时变滤波器也估计协方差errcov估计误差 y - y e 在每一个样本。绘制它，看看您的滤波器是否达到稳定状态(如您预期的平稳输入噪声)。

次要情节(211)情节(t, errcov) ylabel (“柯伐合金错误”）

从这个协方差图中，你可以看到输出协方差在大约5个样本中确实达到了稳定状态。从那时起，时变滤波器的性能就和稳态滤波器一样了。

与从实验数据得出的估计误差协方差进行比较：

ester=y-ye；EstErrCov=sum（ester.*ester）/长度（ester）
埃斯特尔科夫=0.4934

该值小于理论值errcov并与稳态设计得到的值接近。

最后，请注意最终值 米 ［ n ］ 以及稳态值米创新收益矩阵的平均值是一致的。

锰
锰=3×10.3798 0.0817 -0.2570
米
M=3×10.3798 0.0817 -0.2570

参考书目

[1] Grimble M.J,鲁棒工业控制:多项式系统的优化设计方法，Prentice Hall，1994年，第261页和第443-456页。

相关的话题

控制系统工具箱文档
万博1manbetx
了解如何自动调整PID控制器增益

了解如何自动调整PID控制器增益

下载代码示例