卡尔曼滤波

打开实时脚本

这个例子展示了如何执行卡尔曼滤波。方法设计一个稳态滤波器卡尔曼命令。然后，您模拟系统，以显示它如何减少测量噪声的误差。这个例子还展示了如何实现时变滤波器，这对于具有非平稳噪声源的系统非常有用。

稳态卡尔曼滤波器

考虑以下带有高斯噪声的离散植物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 w ［ 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$ ．(有关如何选择这些增益的详细信息，请参见卡尔曼．）

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

设计过滤器

您可以使用卡尔曼函数来设计稳态卡尔曼滤波器。这个函数决定了最佳稳态滤波器增益米基于过程噪声协方差对某一特定工厂进行分析问和传感器噪声协方差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;

对于本例，设置 $G ＝ 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．设置采样时间为－1将植物标记为离散(没有特定的取样时间)。

Ts = -1;sys = ss ([B], C, D, Ts,“InputName”, {“u”' w '}，“OutputName”，“y”）;%植物动态和添加输入噪声w

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

Q = 2.3;R = 1;

使用卡尔曼命令来设计筛选器。

[kalmf,L，~，Mx,Z] = kalman(sys,Q,R);

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

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

Kalmf = Kalmf (1，:);

使用滤镜

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

要模拟这个系统，请使用sumblk为测量噪声创建一个输入v．然后,用连接加入sys和卡尔曼滤波器，这样u是共享的输入和嘈杂的工厂输出y输入到其他过滤器输入。结果是一个有输入的仿真模型w，v,u和输出欧美(正确回答)和叶(过滤或估计的反应 $\hat{y}$ )．的信号欧美而且叶分别是装置和滤波器的输出。

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

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

T = (0:100)';U = sin(t/5);

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

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

最后，模拟响应使用lsim．

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

lsim在输出处生成响应欧美将ye加到w，v,u．提取欧美并计算测量的响应。

Yt = out(:，1);真实响应百分比Ye = out(:，2);过滤响应百分比Y = yt + v;测量反应百分比

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

CLF subplot(211)， plot(t,yt，“b”, t,你们,“r——”),包含(“样本数量”), ylabel (“输出”)标题(“卡尔曼滤波响应”)传说(“真正的”，“过滤”) subplot(212)， plot(t,y -y，‘g’t yt-ye“r——”),包含(“样本数量”), ylabel (“错误”)传说(“真实——经过衡量”，'True -过滤'）

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

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

MeasErr = yt-yt;MeasErrCov = sum(MeasErr.*MeasErr)/length(MeasErr)

MeasErrCov = 0

EstErr = ye -ye;EstErr cov = sum(EstErr.*EstErr)/length(EstErr)

estercov = 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®中实现时变卡尔曼滤波器卡尔曼滤波器块。有关演示该块使用的示例，请参见时变卡尔曼滤波状态估计．对于本例，在MATLAB®中实现时变滤波器。

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

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

接下来，在a中实现递归过滤器更新方程为循环。

P = b * q * b ';初始误差协方差X = 0 (3,1);状态的初始条件Ye = 0(长度(t)，1);Ycov = 0(长度(t)，1);Errcov = 0(长度(t)，1);为i = 1:长度(t)%测量更新Mxn = P*C'/(C*P*C'+R);x = x + Mxn*(y(i)-C*x);% x [n | n]P =(眼(3)-Mxn*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,欧美,“b”, t,你们,“r——”)包含(“样本数量”), ylabel (“输出”)标题(时变卡尔曼滤波器响应)传说(“真正的”，“过滤”) subplot(212)， plot(t,y -y，‘g’t yt-ye“r——”),包含(“样本数量”), ylabel (“错误”)传说(“真实——经过衡量”，'True -过滤'）

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

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

图plot(t,errcov)“样本数量”), ylabel (误差协方差的)，

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

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

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

MeasErr = yt - y;MeasErrCov = sum(MeasErr.*MeasErr)/length(MeasErr)

MeasErrCov = 0.9871

EstErr = yt - ye;EstErr cov = sum(EstErr.*EstErr)/length(EstErr)

estercov = 0.3479

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

Mx,麦根

Mx =3×10.5345 0.0101 -0.4776

麦根=3×10.5345 0.0101 -0.4776

另请参阅

卡尔曼

外部网站

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

卡尔曼滤波

稳态卡尔曼滤波器

设计过滤器

使用滤镜

时变卡尔曼滤波器设计

另请参阅

相关的话题

外部网站