卡尔曼滤波
这个例子展示了如何执行卡尔曼滤波。首先,你设计一个稳态过滤使用卡尔曼
命令。然后,模拟系统来显示它减少了误差从测量噪声。这个示例还展示了如何实现一个时变滤波器,并将其用于系统的非平稳噪声来源。
稳态卡尔曼滤波器
考虑下面的离散植物与高斯噪声w在输入和测量噪声v输出:
目标是设计一个卡尔曼滤波器来估计真正的工厂产出 基于噪声测量 。这个稳态卡尔曼滤波器使用下列公式估算。
时间更新:
测量更新:
在这里,
是估计的 ,鉴于过去测量 。
和 估计状态和测量值,更新基于过去的测量 。
和 最优创新收益,选择最小化稳态协方差的估计误差,考虑到噪声协方差吗 , , 。(关于这些收益是如何选择详情,请参阅
卡尔曼
。)
(这些更新方程描述当前的
类型的估计量。信息之间的区别当前的
估计和延迟
估计,看卡尔曼
。)
设计滤波器
您可以使用卡尔曼
设计这种稳态卡尔曼滤波函数。这个函数确定最优稳态滤波增益米对于一个特定的植物基于过程噪声协方差问和传感器噪声协方差R你提供的。对于这个示例,使用以下值的整数矩阵。
A = [1.1269 -0.4940 0.1129 1.0000 1.0000 0 0 0 0];B = (-0.3832 0.5919 0.5191);C = (1 0 0);D = 0;
对于这个示例,集 ,这意味着这个过程噪音w添加剂输入噪声。同时,设置 ,也就是说,输入噪声w没有直接影响输出y。这些假设收益率简单工厂模式:
当H= 0时,它可以显示
(见卡尔曼
)。在一起,这些假设还简化卡尔曼滤波器的更新方程。
时间更新:
测量更新:
设计这种滤波器,首先创建植物模型的输入w
。样品时间1
将植物标记为离散的(没有特定的样品时间)。
Ts = 1;sys = ss ([B], C, D, Ts,“InputName”,{“u”' w '},“OutputName”,“y”);%植物动力学和添加剂输入噪声w
过程噪声协方差问
和传感器噪声协方差R
值大于零,你通常从研究或测量系统。对于这个示例,指定以下值。
Q = 2.3;R = 1;
使用卡尔曼
命令来设计滤波器。
[kalmf, L, ~, Mx, Z] =卡尔曼(sys, Q, R);
这个命令设计卡尔曼滤波器,kalmf
实现time-update状态空间模型、测量校正方程。过滤输入装置输入u和嘈杂的工厂产出y。第一个输出的kalmf
是估计
真正的工厂产出的,剩下的输出状态估计
。
对于这个示例,丢弃状态估计,只保留第一个输出, 。
:kalmf = kalmf (1);
使用过滤器
这个过滤器的工作原理,生成一些数据和过滤后的响应与真正的植物响应进行比较。完整的系统如下图所示。
来模拟该系统,使用一个sumblk
创建一个输入测量噪声v
。然后,用连接
加入sys
卡尔曼滤波器在一起这样u
是一个共享的输入和嘈杂的工厂产出y
提要到其他滤波器输入。结果是一个与输入仿真模型w
,v
,u
和输出欧美
(真正的反应)叶
(过滤或估计响应
)。的信号欧美
和叶
植物和过滤器的输出,分别。
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
生成响应的输出欧美
你们的投入应用w
,v
,u
。提取欧美
你们渠道和计算测量的响应。
欧美= (:1);%真实反应你们= (:,2);%过滤反应y =次+ v;%响应测量
比较真实的反应和过滤后的反应。
clf次要情节(211),情节(t,欧美,“b”,t,你们,“r——”),包含(样品的数量),ylabel (“输出”)标题(“卡尔曼滤波器响应”)传说(“真正的”,“过滤”)次要情节(212)、图(t yt-y‘g’t yt-ye“r——”),包含(样品的数量),ylabel (“错误”)传说(“真正衡量”,“真-过滤”)
第二个图显示,卡尔曼滤波器可以减少错误欧美- y
由于测量噪声。确认这个减少,计算误差的协方差过滤之前过滤(测量误差协方差),之后(估计误差协方差)。
MeasErr = yt-yt;MeasErrCov = (MeasErr。* MeasErr)之和/长度(MeasErr)
MeasErrCov = 0
EstErr = yt-ye;EstErrCov = (EstErr。* EstErr)之和/长度(EstErr)
EstErrCov = 0.3479
时变卡尔曼滤波器设计
前面的设计假定噪声协方差不随时间变化。时变卡尔曼滤波器可以表现良好,即使噪声协方差不是静止的。
时变卡尔曼滤波器具有以下更新方程。误差协方差时变的滤波器
和创新获得
可以随时间。您可以修改时间和测量更新方程占时间变化如下。(见卡尔曼
关于这些表达式的更多细节)。
时间更新:
测量更新:
你可以在仿真软件实现时变卡尔曼滤波器®使用金宝app卡尔曼滤波器块。块为例演示使用,明白了使用时变卡尔曼滤波器的状态估计。对于这个示例,实现MATLAB®的时变滤波器。
创建时变卡尔曼滤波器,首先生成响应嘈杂的工厂。模拟植物响应输入信号u
和过程噪声w
之前定义的。然后,添加测量噪声v
模拟真实的反应欧美
获得的响应y
。在这个例子中,噪声的协方差向量w
和v
不随时间改变。不过,您可以使用相同的过程的非平稳噪声。
欧美= lsim (sys, [u w]);y =次+ 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 +麦根* (y (i) - c * x);% x [n | n]P =(眼(3)麦根* C) * P;% P [n | n]你们(i) = C * x;errcov (i) = C * P * C ';%的时间更新x = x + B * * u(我);% x [n + 1 | n]P = * P *“B + B * Q *”;% P [n + 1 | n]结束
比较真实的反应和过滤后的反应。
次要情节(211),情节(t,欧美,“b”,t,你们,“r——”)包含(样品的数量),ylabel (“输出”)标题(的反应与时变卡尔曼滤波器)传说(“真正的”,“过滤”)次要情节(212)、图(t yt-y‘g’t yt-ye“r——”),包含(样品的数量),ylabel (“错误”)传说(“真正衡量”,“真-过滤”)
时变滤波器还估计输出协方差估计。因为这个例子使用固定输入噪声,输出协方差趋于一个稳定值。情节输出协方差确认过滤器已经达到一个稳定状态。
图绘制(t, errcov)包含(样品的数量),ylabel (误差协方差的),
协方差的情节,可以看出,输出协方差达到一个稳定状态在大约5个样品。从那时起,时变滤波器的稳态性能相同的版本。
在稳态情况下,滤波器可以减少由于测量噪声误差。确认这个减少,计算误差的协方差过滤之前过滤(测量误差协方差),之后(估计误差协方差)。
MeasErr =次- y;MeasErrCov = (MeasErr。* MeasErr)之和/长度(MeasErr)
MeasErrCov = 0.9871
EstErr =次——你们;EstErrCov = (EstErr。* EstErr)之和/长度(EstErr)
EstErrCov = 0.3479
最后,当时变滤波器达到稳态,增益矩阵中的值麦根
匹配计算的卡尔曼
稳态的过滤器。
Mx,麦根
Mx =3×10.5345 0.0101 -0.4776
麦根=3×10.5345 0.0101 -0.4776