卡尔曼滤波器设计
这个例子展示了如何执行卡尔曼滤波。下面分别设计并模拟了稳态滤波器和时变滤波器。
问题描述
给定下面的离散工厂
在哪里
A = [1.1269 -0.4940 0.1129, 1.0000 0, 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
稳态卡尔曼滤波器设计
您可以使用函数kalman设计稳态卡尔曼滤波器。该功能基于过程噪声协方差Q和传感器噪声协方差R确定最佳稳态滤波器MM。
首先指定工厂+噪声模型。注意:设置采样时间为-1，表示该工厂为离散型。
(A，[B]，C,0，-1，“inputname”，{“u”' w '}，'outputname','是'）;
指定过程噪声协方差（Q）：
Q = 2.3;大于0的数字
指定传感器噪声协方差(R):
R = 1;大于0的数字
现在使用方程式设计稳态卡尔曼滤波器
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'，{'是''yv'});
接下来，将植物模型与卡尔曼滤波器并联，指定u为共享输入:
SYS =并联（P，Kalmf，1,1，[]，[]）;
最后，将工厂输出YV连接到滤波器输入YV。注意：YV是SYS的第4个输入，也是其第二输出：
SimModel =反馈(sys, 1、4、2、1);SimModel = SimModel([1 3]，[1 2 3]);% Delete yv form I/O
得到的仿真模型以w,v,u为输入，y,y_e为输出:
SimModel.inputname
ans = 3x1小区数组{'w'} {'v'} {'u'}
SimModel.outputname
ans = 2x1单元阵列{'y'} {'y_e'}
现在可以模拟过滤器行为了。生成一个正弦输入向量(已知):
t =（0：100）';u = sin（t / 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，ye，“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);
滤波前误差的协方差(测量误差):
MEARERRCOV
MEARERRCOV = 0.9871
滤波后误差的协方差(估计误差):
Esterrcov.
EstErrCov = 0.3479
时变卡尔曼滤波器设计
现在，设计一个时变的卡尔曼滤波器来执行相同的任务。即使当噪声协方差没有静止时，也可以更改的卡尔曼滤波器表现良好。但是，对于这个例子，我们将使用静止的协方差。
时变卡尔曼滤波器有以下更新方程。
时间更新：x [n + 1 | n] = ax [n | n] + bu [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);％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）;％x [n | n]P =(眼(3)mn * 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, y,“b”，t，ye，“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);
滤波前误差的协方差(测量误差):
MEARERRCOV
MEARERRCOV = 0.9871
滤波后误差的协方差(估计误差):
Esterrcov.
EstErrCov = 0.3479
验证卡尔曼增益矩阵的稳态值与最终值重合:
M，Mn.
M = 0.5345 0.0101 -0.4776 Mn = 0.5345 0.0101 -0.4776
外部网站
您还可以从以下列表中选择一个网站：
