卡尔曼滤波

打开实时脚本

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

稳态卡尔曼滤波

考虑以下具有高斯噪声的离散对象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．设置采样时间为－１将设备标记为离散的(没有特定的采样时间)。

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

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

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

使用滤镜

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

要模拟这个系统，使用asumblk为测量噪声创建一个输入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 = sqrt(Q)*randn(length(t)，1);v = sqrt(R)*randn(length(t)，1);

最后，使用模拟响应lsim．

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

lsim在输出处生成响应欧美ye的输入w，v,u．提取欧美然后计算测量的响应。

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

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

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

图中包含2个轴对象。轴对象1标题卡尔曼滤波响应，xlabel样本数，ylabel输出包含2个类型为线的对象。这些对象表示True, Filtered。轴对象2与xlabel样本数，ylabel错误包含类型为线的2个对象。这些对象代表真测量，真过滤。

如图2所示，卡尔曼滤波器减小了误差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]$ 以及创新收益 $米_{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不要随时间而改变。但是，您可以对非平稳噪声使用相同的程序。

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

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

P = b * q * b ';初始误差协方差X = 0 (3,1);%状态的初始条件Ye = 0 (length(t)，1);Ycov = 0 (length(t)，1);Errcov = 0 (length(t)，1);为i = 1:长度(t)%测量更新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';%时间更新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 (“错误”)传说(“真实测量”，“真实-过滤”）

图中包含2个轴对象。轴对象1，标题为响应随时间变化的卡尔曼滤波器，xlabel样本数，ylabel输出包含2个类型为line的对象。这些对象表示True, Filtered。轴对象2与xlabel样本数，ylabel错误包含类型为线的2个对象。这些对象代表真测量，真过滤。

时变滤波器还在估计过程中估计输出协方差。由于本例使用平稳输入噪声，因此输出协方差趋向于稳态值。绘制输出协方差以确认滤波器已达到稳定状态。

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

图包含一个轴对象。带有xlabel Number of Samples, ylabel Error Covariance的axes对象包含一个类型为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

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

Mx,麦根

Mx =3×10.5345 0.0101 -0.4776

麦根=3×10.5345 0.0101 -0.4776

另请参阅

卡尔曼

外部网站

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

卡尔曼滤波

稳态卡尔曼滤波

设计过滤器

使用滤镜

时变卡尔曼滤波器设计

另请参阅

相关的话题

外部网站