卡尔曼滤波

打开生活的脚本

这个例子展示了如何执行卡尔曼滤波。首先，设计一个稳态滤波器卡尔曼命令。然后，你模拟系统，以显示它是如何减少误差的测量噪声。这个例子也展示了如何实现一个时变滤波器，它可以有用的系统与非平稳噪声源。

稳态卡尔曼滤波器

考虑以下带有高斯噪声的离散植物w对输入和测量噪声v输出:

$\begin{array}{c} x ［ n + 1 ］＝一个 x ［ n ］ + B u ［ n ］ + G w ［ n ］ \\ y ［ n ］＝ C x ［ n ］ + D u ［ n ］ + H u ［ n ］ + v ［ n ］ \end{array}$

目标是设计一个卡尔曼滤波器来估计真实的植物输出 $y_{t} ［ n ］＝ y ［ n ］ - v ［ n ］$ 基于噪声测量 $y ［ n ］$ ．这个稳态卡尔曼滤波器使用下面的方程来进行估计。

时间更新:

$\hat{x} ［ n + 1 | n ］＝一个 \hat{x} ［ n | n - 1 ］ + 部［ n ］ + 吉瓦［ n ］$

测量更新:

$\begin{array}{l} x_{}^{ˆ} ［ n | n ］＝ x_{}^{ˆ} ［ n | n - 1 ］ + 米_{x} （ y ［ n ］ - C x_{}^{ˆ} ［ n | n - 1 ］ - D u ［ n ］） \\ y_{}^{ˆ} ［ n | n ］＝ C x_{}^{ˆ} ［ n | n - 1 ］ + D u ［ n ］ + 米_{y} （ y ［ n ］ - C x_{}^{ˆ} ［ n | n - 1 ］ - D u ［ n ］） \end{array}$

在这里,

$\hat{x} ［ n | n - 1 ］$ 为 $x ［ n ］$ ，根据过去的测量结果 $y ［ n - 1 ］$ ．
$\hat{x} ［ n | n ］$ 和 $\hat{y} ［ n | n ］$ 估算的状态值和测量值是否根据上次的测量值进行更新 $y ［ n ］$ ．
$米_{x}$ 和 $米_{y}$ 在给定噪声协方差的情况下，是否选择了最小化估计误差的稳态协方差的最优创新增益 $E （ w ［ n ］ {w ［ n ］}^{T} ）＝问$ ， $E （ v ［ n ］ {v ［ n ］}^{T} ）＝ R$ , $N ＝ E （ w ［ n ］ {v ［ n ］}^{T} ）＝ 0$ ．(有关如何选择这些增益的详细信息，请参见卡尔曼．）

(这些更新方程描述了一个当前的类型的估计量。来了解两者的区别当前的估计和延迟估计,看卡尔曼．）

设计滤波器

你可以使用卡尔曼函数来设计稳态卡尔曼滤波器。这个函数决定了最佳稳态滤波器增益米基于过程噪声协方差对某一特定设备进行分析问传感器噪声协方差R你提供的。对于本例，使用下列值作为该植物的状态空间矩阵。

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

对于本例，setG＝B，意味着过程噪音w为附加输入噪声。同时,设置H= 0，表示输入噪声w对产量没有直接影响y．这些假设得出了一个更简单的植物模型:

$\begin{array}{c} x ［ n + 1 ］＝一个 x ［ n ］ + B u ［ n ］ + B w ［ n ］ \\ y ［ n ］＝ C x ［ n ］ + v ［ n ］ \end{array}$

当H= 0时，可以证明 $米_{y} ＝ C 米_{x}$ (见卡尔曼）.同时，这些假设也简化了卡尔曼滤波器的更新方程。

时间更新:

$\hat{x} ［ n + 1 | n ］＝一个 \hat{x} ［ n | n - 1 ］ + 部［ n ］ + Bw ［ n ］$

测量更新:

$\begin{array}{l} x_{}^{ˆ} ［ n | n ］＝ x_{}^{ˆ} ［ n | n - 1 ］ + 米_{x} （ y ［ n ］ - C x_{}^{ˆ} ［ n | n - 1 ］） \\ y_{}^{ˆ} ［ n | n ］＝ C x_{}^{ˆ} ［ n | n ］ \end{array}$

要设计这个过滤器，首先要创建带有输入的植物模型w．将示例时间设置为－１将植物标记为离散的(没有具体的采样时间)。

Ts = 1;(A，[B, B]，C,D, t))“InputName”, {“u”' w '}，“OutputName”，“y”）;植物动态和附加输入噪声

过程噪声协方差问传感器噪声协方差R通常从系统的研究或测量中获得的大于零的值。对于本例，指定以下值。

Q = 2.3;R = 1;

使用卡尔曼命令来设计过滤器。

[kalmf, L, ~, Mx, Z] =卡尔曼(sys, Q, R);

这个命令设计了卡尔曼滤波器，kalmf，状态空间模型，实现时间更新和测量更新方程。过滤器的输入是植物的输入u和嘈杂的工厂输出y．的第一个输出kalmf是估计 $\hat{y}$ 的真实产量，其余的产量是状态估计 $\hat{x}$ ．

对于本例，放弃状态估计，只保留第一个输出， $\hat{y}$ ．

: kalmf = kalmf (1);

使用过滤器

为了了解这个过滤器是如何工作的，生成一些数据，并将过滤后的响应与真正的植物响应进行比较。完整的系统如下图所示。

要模拟这个系统，使用sumblk为测量噪声创建一个输入v．然后,用连接加入sys和卡尔曼滤波器一起u是否共享输入和嘈杂的工厂输出yfeed到另一个过滤器输入。结果是一个带有输入的仿真模型w，v,u和输出欧美(真正的反应)叶(过滤或估计的反应 $\hat{y}$ ）.的信号欧美和叶分别为植物和过滤器的输出。

sys。我nputName = {“u”，' w '};sys。OutputName = {“次”};vIn = sumblk (“y =次+ v”）;kalmf。我nputName = {“u”，“y”};kalmf。OutputName =“叶”；kalmf SimModel =连接(sys, vIn, {“u”，' w '，“v”}，{“次”，“叶”})；

为了模拟滤波器的行为，生成一个已知的正弦输入向量。

t =(0:100)”;u =罪(t / 5);

使用相同的噪声协方差值生成过程噪声和传感器噪声向量问和R你用来设计过滤器的。

rng (10,“旋风”）;w = sqrt (Q) * randn(长度(t), 1);v = sqrt (R) * randn(长度(t), 1);

最后，使用lsim．

= lsim (SimModel, [u, w, v]);

lsim在输出处生成响应欧美而ye是应用于w，v,u．提取欧美和ye通道，计算测量的响应。

欧美= (:1);%真实反应你们= (:,2);%过滤反应Y = yt + v;%响应测量

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

clf次要情节(211),情节(t,欧美,“b”, t,你们,“r——”),包含(样品的数量), ylabel (“输出”)标题(“卡尔曼滤波器响应”)传说(“真正的”，“过滤”)次要情节(212)、图(t yt-y‘g’t yt-ye“r——”),包含(样品的数量), ylabel (“错误”)传说(“真正衡量”，“真-过滤”）

图中包含2个轴对象。标题为Kalman Filter Response的坐标轴对象1包含2个类型为line的对象。这些对象代表True, Filtered。axis对象2包含2个类型为line的对象。这些对象代表真实测量，真实过滤。

如第二幅图所示，卡尔曼滤波器减少了误差欧美- y由于测量噪声。为了确认这种减少，计算滤波前的误差协方差(测量误差协方差)和滤波后的误差协方差(估计误差协方差)。

MeasErr = yt-yt;MeasErrCov = (MeasErr。* MeasErr)之和/长度(MeasErr)

MeasErrCov = 0

EstErr = yt-ye;EstErrCov = (EstErr。* EstErr)之和/长度(EstErr)

EstErrCov = 0.3479

时变卡尔曼滤波器设计

先前的设计假设噪声协方差不随时间变化。时变卡尔曼滤波器即使在噪声协方差不稳定的情况下也能有很好的性能。

时变卡尔曼滤波器的更新方程如下:在时变滤波器中，两者的误差都是协方差的 $P ［ n ］$ 创新的收益 $米_{x} ［ n ］$ 可以随时间而变化。您可以修改时间和测量更新方程来考虑时间的变化，如下所示。(见卡尔曼以了解这些表达式的更多细节。)

时间更新:

$\begin{array}{l} x_{}^{ˆ} ［ n + 1 | n ］＝一个 x_{}^{ˆ} ［ n | n ］ + B u ［ n ］ + B w ［ n ］ \\ P ［ n + 1 | n ］＝一个 P ［ n | n ］ {一个}^{T} + B 问 B^{T} \end{array}$

测量更新:

$\begin{array}{l} x_{}^{ˆ} ［ n | n ］＝ x_{}^{ˆ} ［ n | n - 1 ］ + 米_{x} ［ n ］（ y ［ n ］ - C x_{}^{ˆ} ［ n | n - 1 ］） \\ 米_{x} ［ n ］＝ P ［ n | n - 1 ］ C^{T} {（ C P ［ n | n - 1 ］ C^{T} + R ［ n ］）}^{- 1} \\ P ［ n | n ］＝（我 - 米_{x} ［ n ］ C ） P ［ n | n - 1 ］ \\ y_{}^{ˆ} ［ n | n ］＝ C x_{}^{ˆ} ［ n | n ］ \end{array}$

您可以在Simulink®中使用金宝app卡尔曼滤波器块。有关演示该块用法的示例，请参见基于时变卡尔曼滤波的状态估计．针对本例，在MATLAB®中实现时变滤波器。

为了建立时变卡尔曼滤波器，首先，产生噪声植物响应。模拟植物对输入信号的响应u和过程噪声w之前定义的。然后，添加测量噪声v模拟的真实响应欧美以获得噪声响应y．在这个例子中，噪声向量的协方差w和v不要随时间而改变。但是，对于非平稳噪声也可以使用相同的方法。

Yt = lsim(sys，[u w]);Y = yt + v;

中实现递归过滤器更新方程为循环。

P = B * * B”;%初始误差协方差x = 0 (3,1);%状态的初始条件你们= 0(长度(t), 1);ycov = 0(长度(t), 1);errcov = 0(长度(t), 1);为i = 1:长度(t)%测量更新麦根= P * C C / P (C * *”+ R);x = x + Mxn*(y(i)-C*x);% x [n | n]P =(眼(3)麦根* C) * P;% P [n | n]你们(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,欧美,“b”, t,你们,“r——”)包含(样品的数量), ylabel (“输出”)标题(“时变卡尔曼滤波响应”)传说(“真正的”，“过滤”)次要情节(212)、图(t yt-y‘g’t yt-ye“r——”),包含(样品的数量), ylabel (“错误”)传说(“真正衡量”，“真-过滤”）

图中包含2个轴对象。标题为“响应时变卡尔曼滤波”的坐标轴对象1包含2个类型为line的对象。这些对象代表True, Filtered。axis对象2包含2个类型为line的对象。这些对象代表真实测量，真实过滤。

在估计过程中，时变滤波器也估计输出协方差。因为这个例子使用平稳的输入噪声，输出协方差趋向于一个稳定状态值。绘制输出协方差以确认滤波器已达到稳定状态。

图绘制(t, errcov)包含(样品的数量), ylabel (误差协方差的），

图中包含一个轴对象。axis对象包含一个类型为line的对象。

从协方差图中可以看出，输出协方差在大约5个样本中达到稳定状态。从那时起，时变滤波器的性能与稳态滤波器相同。

在稳态情况下，滤波器减少了测量噪声引起的误差。为了确认这种减少，计算滤波前的误差协方差(测量误差协方差)和滤波后的误差协方差(估计误差协方差)。

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

MeasErrCov = 0.9871

EstErr = yt - ye;EstErrCov = (EstErr。* EstErr)之和/长度(EstErr)

EstErrCov = 0.3479

最后，当时变滤波器达到稳态时，得到增益矩阵中的值麦根匹配由卡尔曼用于稳态滤波器。

Mx,麦根

Mx =3×10.5345 0.0101 -0.4776

麦根=3×10.5345 0.0101 -0.4776

另请参阅

卡尔曼

外部网站

理解卡尔曼滤波器-MATLAB视频系列

卡尔曼滤波

稳态卡尔曼滤波器

设计滤波器

使用过滤器

时变卡尔曼滤波器设计

另请参阅

相关的话题

外部网站

控制系统工具箱文档

金宝app

学习如何自动调整PID控制器增益