卡尔曼滤波器设计
这个例子展示了如何执行卡尔曼滤波。设计并仿真了稳态滤波器和时变滤波器。
问题描述
给定以下离散工厂
在哪里
A = [1.1269 - 4940 0.1129, 1.0000 00, 0 1.0000 0];B = [-0.3832 0.5919 0.5191];C = [1 0 0];D = 0;
设计卡尔曼滤波器来估计基于所述噪声测量YV [N] = C X [n]的+ V [n]的输出y
稳态卡尔曼滤波器设计
您可以使用该功能卡尔曼设计一个稳态卡尔曼滤波器。这个函数确定基于过程噪声协方差Q和传感器噪声协方差R的最佳稳态滤波器增益中号
首先指定工厂+噪音模型。注意:将采样时间设置为-1，将工厂标记为离散的。
植物= ss(A，[B B]，C,0，-1，“inputname”{“u”' w '}，'outputname','Y'）;
指定处理噪声协方差（Q）：
Q = 2.3;一个大于零的数
指定传感器噪声协方差(R):
R = 1;一个大于零的数
现在设计了一个方程稳态卡尔曼滤波
时间更新:x[n+1|n] = Ax[n|n-1] + Bu[n] + Bw[n]
测量更新：X [N | N] = X [N | N-1] + M（YV [N] - CX [N | N-1]）
其中M =使用KALMAN命令获得的最优创新收益:
[kalmf, L, ~, M, Z] =卡尔曼(植物、Q、R);
卡尔曼滤波器KALMF的第一个输出是工厂的输出估计y_e = Cx[n|n]，其余的输出是状态估计。只保留第一个输出y_e:
:kalmf = kalmf (1);米,%创新获得
M = 0.5345 0.0101 -0.4776
要查看该过滤器是如何工作的，产生一些数据，并比较真实的植物响应过滤的响应：
为了模拟上述系统中，可以分别产生每个部件的响应或产生两者一起。要单独模拟每个，首先使用LSIM与植物，然后用过滤器。下面的示例模拟两者一起。
％首先，建立一个完整的工厂模型，U，W，V作为输入和% y和yv作为输出: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'});
接下来，通过指定u作为共享输入，并行连接工厂模型和卡尔曼滤波器:
SYS =平行（P，kalmf，1,1，[]，[]）;
最后，连接设备输出YV到滤波器输入YV。注：YV是SYS，也第2输出的第4个输入：
SimModel =反馈(sys, 1、4、2、1);SimModel = SimModel([1 3]，[1 2 3]);删除yv表格I/O
得到的仿真模型以w、v、u为输入，y、y_e为输出:
SimModel.inputname
ANS = 3X1单元阵列{ 'W'} { 'V'} { 'U'}
SimModel.outputname
ANS = 2×1单元阵列{ 'Y'} { 'y_e'}
现在可以模拟筛选器行为了。生成一个正弦输入向量(已知):
T =（0：100）';U = SIN（吨/ 5）;
产生过程噪声和传感器噪声矢量:
rng (10,“旋风”）;W = SQRT（Q）* randn（长度（T），1）;V = SQRT（R）* randn（长度（T），1）;
现在，使用模拟的LSIM响应：
= lsim (SimModel, [w, v, u]);y = (: 1);％真实反应你们= (:,2);％响应过滤yv = y + v;%响应测量
比较真实的响应和过滤后的响应:
CLF副区（211），曲线图（T，Y，“b”，T，咋，“r——”），xlabel（“不。样品的），ylabel（“输出”）标题（“卡尔曼滤波器响应”）副区（212），曲线图（T，Y-YV，‘g’t y-ye“r——”），xlabel（“不。样品的），ylabel（'错误')
如图2所示，卡尔曼滤波降低了测量噪声引起的误差y-yv。为了确认这一点，比较误差协方差:
MeasErr = y-yv;MeasErrCov = (MeasErr。* MeasErr)之和/长度(MeasErr);EstErr = y-ye;EstErrCov = (EstErr。* EstErr)之和/长度(EstErr);
滤波前误差协方差(测量误差):
MeasErrCov
MeasErrCov = 0.9871
滤波后误差协方差(估计误差):
EstErrCov
EstErrCov = 0.3479
时变卡尔曼滤波器设计
现在，设计时变卡尔曼滤波器来执行相同的任务。即使当噪声协方差不是静止的时变卡尔曼滤波器可以表现良好。然而在这个例子中，我们将使用固定的协方差。
时变卡尔曼滤波器的更新方程如下:
时间更新：X [n + 1个| N] = AX [N | N] +卜[N] + BW [n]的
P[n+1|n] = AP[n|n]A' + B*Q*B'
测量更新:x [n | n] = [n | n - 1] + M [n](青年志愿[n] -残雪[n | n - 1]) 1 M [n] = P [n | n - 1] C ' (CP + R (n | n - 1) C”)
P [N | N] =（I-M [n]的C）P [N | N-1]
首先，生成有噪声的植物响应:
sys = ss (A, B, C, D, 1);y = lsim (sys, u + w);％（重量）=处理噪声yv = y + v;％V = MEAS。噪声
接下来，在FOR循环中实现过滤器递归:
P = B * Q * B';%初始误差协方差X =零（3,1）;％的状态初始条件你们= 0(长度(t), 1);ycov = 0(长度(t), 1);errcov = 0(长度(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);% x [n + 1 | n]P = A*P*A' + B*Q*B';% P [n + 1 | n]结束
现在，比较真实的响应和过滤后的响应:
次要情节(211),情节(t, y,“b”，T，咋，“r——”），xlabel（“不。样品的），ylabel（“输出”）标题（时变卡尔曼滤波器响应）副区（212），曲线图（T，Y-YV，‘g’t y-ye“r——”），xlabel（“不。样品的），ylabel（'错误')
时变滤波器在估计过程中估计输出协方差。绘制输出协方差，看看是否滤波器已达到稳态(正如我们预期的平稳输入噪声):
次要情节(211)情节(t, errcov) ylabel (“错误COVAR”),
从协方差图中可以看出，在大约5个样本中，输出协方差确实达到了一个稳定的状态。从那时起，时变滤波器具有与稳态滤波器相同的性能。
比较协方差错误：
MeasErr = y-yv;MeasErrCov = (MeasErr。* MeasErr)之和/长度(MeasErr);EstErr = y-ye;EstErrCov = (EstErr。* EstErr)之和/长度(EstErr);
滤波前误差协方差(测量误差):
MeasErrCov
MeasErrCov = 0.9871
滤波后误差协方差(估计误差):
EstErrCov
EstErrCov = 0.3479
验证卡尔曼增益矩阵的稳态和终值重合:
男，锰
M = 0.5345 0.0101 -0.4776的Mn = 0.5345 0.0101 -0.4776
