卡尔曼滤波

开立真实脚本

本案例研究说明卡尔曼滤波器的设计和仿真。这两种稳态和时变卡尔曼滤波器被考虑。

装置动态

考虑加性高斯噪声离散厂 $w ^[ñ]$ 在输入 $ü [ñ]$ ：

$\begin{array}{l} X [ñ + 1] = 一个 X [ñ] + 乙（ ü [ñ] + w ^[ñ] ） \\ ÿ [ñ] = C X [ñ] 。 \end{array}$

此外，让 $ÿ_{v} [ñ]$ 是输出的噪声测量结果 $ÿ [ñ]$ 与 $v [ñ]$ 表示测量噪声：

$ÿ_{v} [ñ] = ÿ [ñ] + v [ñ] 。$

以下矩阵表示植物的动力学。

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

离散卡尔曼滤波

稳态卡尔曼滤波器对于这个问题的方程给出如下。

测量更新：

$X_{}^{} [ñ | ñ] = X_{}^{} [ñ | ñ - 1] + 中号（ ÿ_{v} [ñ] - C X_{}^{} [ñ | ñ - 1] ）$

更新时间：

$X_{}^{} [ñ + 1 | ñ] = 一个 X_{}^{} [ñ | ñ] + 乙 ü [ñ]$

在这些等式：

$X_{}^{} [ñ | ñ - 1]$ 是的估计 $X [ñ]$ 鉴于过去的测量高达 $ÿ_{v} [ñ - 1]$ 。
$X_{}^{} [ñ | ñ]$ 基于上次测量更新估计 $ÿ_{v} [ñ]$ 。

鉴于目前的估计 $X_{}^{} [ñ | ñ]$ 中，时间更新在预测下一采样的状态值ñ+ 1（一一步向前预测器）。测量更新，然后调节基于新的测量这个预测 $ÿ_{v} [ñ + 1]$ 。校正项是创新的一个功能，即，所测量的值和预测值之间的差异 $ÿ [ñ + 1]$ 。这种差异由下式给出：

$ÿ_{v} [ñ + 1] - C X_{}^{} [ñ + 1 | ñ]$

处的创新增益M被选择以最小化估算误差的稳态协方差，考虑到噪声协方差：

$Ë （ w ^[ñ] w ^{[ñ]}^{Ť} ） = Q Ë （ v [ñ] v {[ñ]}^{Ť} ） = [R ñ = Ë （ w ^[ñ] v {[ñ]}^{Ť} ） = 0$

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

$\begin{array}{l} X_{}^{} [ñ + 1 | ñ] = 一个（一世 - 中号 C ） X_{}^{} [ñ | ñ - 1] + [\begin{array}{cccccccccccccccccccc} 乙 & 一个中号 \end{array}] [\begin{array}{cccccccccccccccccccc} ü [ñ] \\ ÿ_{v} [ñ] \end{array}] \\ ÿ_{}^{} [ñ | ñ] = C （一世 - 中号 C ） X_{}^{} [ñ | ñ - 1] + C 中号 ÿ_{v} [ñ] 。 \end{array}$

这个滤波器产生的最佳估计 $ÿ_{}^{} [ñ | ñ]$ 的 $ÿ_{ñ}$ 。需要注意的是过滤状态 $X_{}^{} [ñ | ñ - 1]$ 。

稳态设计

您可以设计稳态与功能上述卡尔曼滤波器卡尔曼。首先指定与过程噪声的工厂模型：

$\begin{array}{cccccccccccccccccccc} X [ñ + 1] = 一个 X [ñ] + 乙 ü [ñ] + 乙 w ^[ñ] \\ ÿ [ñ] = C X [ñ] \end{array}$

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

以下命令指定这个工厂模型。采样时间被设定为-1，标记模型中作为离散的而没有指定的采样时间。

植物= SS（A，[B B]，C，0，-1，'inputname'{'U''W'}，'outputname'，'Y'）;

假如说Q=[R= 1，设计的离散卡尔曼滤波器。

Q = 1;R = 1;[kalmf，L，P，M] =卡尔曼（植物，Q，R）;

此命令返回一个状态空间模型kalmf过滤器，以及创新增益中号。

中号

M =3×10.3798 0.0817 -0.2570

的输入kalmf是ü和 $ÿ_{v}$ 和。输出是植物输出和状态估计值， $ÿ_{Ë} = ÿ_{}^{} [ñ | ñ]$ 和 $X_{}^{} [ñ | ñ]$ 。

因为你所感兴趣的输出估计 $ÿ_{Ë}$ 中，选择的第一输出kalmf并丢弃其余部分。

kalmf = kalmf（1，:);

要查看过滤器是如何工作的，产生一些输入数据和随机噪声和比较过滤响应 $ÿ_{Ë}$ 与真实反应ÿ。你要么可以单独生成每个响应，或者产生两者一起。分别模拟每个响应，使用lsim单独用第一植物，然后用植物和滤波器一起挂接。联合仿真替代下一详述。

下面示出了框图如何生成true和滤波输出。

您可以构建该框图与功能的状态空间模型平行和反馈。首先建立了一个完整的工厂模型ü，w ^，v作为输入，并ÿ和 $ÿ_{v}$ （测量）为输出。

A = A;B = [B B 0 * B];C = [C; C];d = [0 0 0 0 0 1];P = SS（A，B，C，d，-1，'inputname'{'U''W''V'}，'outputname'{'Y''YV'}）;

然后用平行以形成以下说明的并联连接。

SYS =平行（P，kalmf，1,1，[]，[]）;

最后，通过连接电站输出关闭传感器回路 $ÿ_{v}$ 到滤波器输入 $ÿ_{v}$ 正反馈。

SimModel =反馈（SYS，1,4,2,1）;围绕输入＃4和＃输出2％闭环SimModel = SimModel（[1 3]，[1 2 3]）;从％我删除YV / O列表

得到的仿真模型w ^，v，ü作为输入，并ÿ和 $ÿ_{Ë}$ 为输出。查看InputName和OutputName性能验证。

SimModel.InputName

ANS =3X1细胞{ 'W'} { 'V'} { 'U'}

SimModel.OutputName

ANS =2×1单元{ 'Y'} { 'y_e'}

您现在可以模拟滤波器行为。产生正弦输入ü和过程和测量噪声向量w ^和v。

T = [0：100]';U = SIN（吨/ 5）;N =长度（T）;RNG默认W = SQRT（Q）* randn（N，1）;V = SQRT（R）* randn（N，1）;

模拟的响应。

[出来，X] = lsim（SimModel，[W，V，U，C]）;Y = OUT（：，1）;％真实反应咋= OUT（：，2）;％响应过滤YV = Y + V;％测量的响应

图形比较真实和过滤响应。

副区（211），曲线图（T，Y，' - '，T，咋，' - '），xlabel（'没有。样本），ylabel（“输出”）标题（“卡尔曼滤波器响应”）副区（212），曲线图（T，Y-YV，' - '。，T，Y业，' - '），xlabel（'没有。样本），ylabel（'错误'）

第一个图显示的真实反应ÿ（虚线）和滤波后的输出 $ÿ_{Ë}$ （实线）。第二曲线的测定误差（点划线）与所述估计误差（固体）进行比较。该图显示，噪声水平已显著减少。这是通过计算协方差错误证实。滤波（测量误差）之前的误差协方差是：

MEASERR = Y-YV;MeasErrCov =总和（MEASERR。* MEASERR）/长度（MEASERR）

MeasErrCov = 0.9992

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

EstErr = Y业;EstErrCov =总和（EstErr。* EstErr）/长度（EstErr）

EstErrCov = 0.4944

时变卡尔曼滤波

随时间变化的卡尔曼滤波器是稳态滤波器，用于随时间变化的系统或与不稳定噪声协方差LTI系统的推广。

考虑下面的设备状态和测量方程。

$\begin{array}{rcl} X [ñ + 1] & = & 一个 X [ñ] + 乙 ü [ñ] + G w ^[ñ] \\ ÿ_{v} [ñ] & = & C X [ñ] + v [ñ] 。 \end{array}$

随时间变化的卡尔曼滤波器由以下递归式给出：

测量更新：

$\begin{array}{rcl} X_{}^{} [ñ | ñ] & = & X_{}^{} [ñ | ñ - 1] + 中号 [ñ] （ ÿ_{v} [ñ] - C X_{}^{} [ñ | ñ - 1] ） \\ 中号 [ñ] & = & P [ñ | ñ - 1] C^{Ť} （ [R [ñ] + C P [ñ | ñ - 1] C^{Ť} ）^{- 1} \\ P [ñ | ñ] & = & （一世 - 中号 [ñ] C ） P [ñ | ñ - 1] 。 \end{array}$

更新时间：

$\begin{array}{rcl} X_{}^{} [ñ + 1 | ñ] & = & 一个 X_{}^{} [ñ | ñ] + 乙 ü [ñ] \\ P [ñ + 1 | ñ] & = & 一个 P [ñ | ñ] {一个}^{Ť} + G Q [ñ] G^{Ť} 。 \end{array}$

这里， $X_{}^{} [ñ | ñ - 1]$ 和 $X_{}^{} [ñ | ñ]$ 如前所述。另外：

$\begin{array}{l} Q [ñ] = Ë （ w ^[ñ] w ^{[ñ]}^{Ť} ） \\ [R [ñ] = Ë （ v [ñ] v {[ñ]}^{Ť} ） \\ P [ñ | ñ] = Ë （ {X [ñ] - X [ñ | ñ]} {X [ñ] - X [ñ | ñ]}^{Ť} ） \\ P [ñ | ñ - 1] = Ë （ {X [ñ] - X [ñ | ñ - 1]} {X [ñ] - X [ñ | ñ - 1]}^{Ť} ）。 \end{array}$

为简单起见，表示状态空间矩阵的时间依赖性下标已被丢弃。

由于初始条件 $X [1 | 0]$ 和 $P [1 | 0]$ ，你可以遍历这些方程进行过滤。你必须同时更新状态估计 $X [ñ | 。]$ 和误差协方差矩阵 $P [ñ | 。]$ 在每个时间样本。

时变设计

为了实现这些过滤器递归，首先产生噪声的输出测量。在使用过程中噪音w ^及测量噪声v以前生成的。

SYS = SS（A，B，C，0，-1）;Y = lsim（SYS，U + W）;YV = Y + V;

假定以下初始条件：

$\begin{array}{cccccccccccccccccccc} X [1 | 0] = 0 ， & P [1 | 0] \end{array} = 乙 Q 乙^{Ť}$

实现具有随时间变化的过滤器对于环。

P = B * Q * B';％初始误差协方差X =零（3,1）;％的状态初始条件咋=零（长度（T），1）;ycov =零（长度（T），1）;对于I = 1：长度（t）的％测量更新的Mn = P * C '/（C * P * C' + R）;X = X +的Mn *（YV（I）-C * X）;％×[N | N]P =（眼（3）-Mn * C）* P;％P [N | N]咋（ⅰ）= C * X;errcov（1）= C * P * C';％时更新X = A * X + B * U（I）;％×[N + 1 | n]的P = A * P * A '+ B * Q * B';％P [N + 1 | n]的结束

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

副区（211），曲线图（T，Y，' - '，T，咋，' - '）标题（“时变卡尔曼滤波器响应”）xlabel（'没有。样本），ylabel（“输出”）副区（212），曲线图（T，Y-YV，' - '。，T，Y业，' - '）xlabel（'没有。样本），ylabel（“输出”）

第一个图显示的真实反应ÿ（虚线）和滤波后的响应 $ÿ_{Ë}$ （实线）。第二曲线的测定误差（点划线）与所述估计误差（固体）进行比较。

随时间变化的过滤器还估计协方差errcov估计误差 $ÿ - ÿ_{Ë}$ 在各样品中。绘制它，看看你的过滤器达到稳定状态（如你期望与静止的输入噪声）。

副区（211）情节（T，errcov）ylabel（“错误COVAR”）

从这个协方差的情节，你可以看到输出协方差确实在达到稳定状态约五个样本。从此，您的随时间变化的过滤器具有相同的性能稳定状态的版本。

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

EstErr = Y  - 业;EstErrCov =总和（EstErr。* EstErr）/长度（EstErr）

EstErrCov = 0.4934

该值比理论值errcov和接近为稳态设计得到的值。

最后，注意终值 $中号 [ñ]$ 和稳态值中号创新增益矩阵不谋而合。

锰

Mn =3×10.3798 0.0817 -0.2570

中号

M =3×10.3798 0.0817 -0.2570

参考书目

[1]格林布尔，M.J.，强大的工业控制：多项式系统优化设计方法，Prentice Hall出版社，1994，页。261页和第443-456。

卡尔曼滤波

装置动态

离散卡尔曼滤波

稳态设计

时变卡尔曼滤波

时变设计

参考书目

相关话题

控制系统工具箱文档

金宝app

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