卡尔曼滤波大泄密

7leaves

卡尔曼滤波大泄密 [复制链接]

一片绿油油的草地上有一条曲折的小径，通向一棵大树。一个要求被提出：从起点沿着小径走到树下。“很简单。” A说，于是他丝毫不差地沿着小径走到了树下。现在，难度被增加了：蒙上眼。“也不难，我当过特种兵。” B说，于是他歪歪扭扭地走到了树………. 旁。“唉，好久不练，生疏了。”“看我的，我有 DIY 的 GPS！” C说，于是他像个醉汉似地走到了树………. 旁。“唉，这个 GPS 软件没做好，漂移太大。”“我来试试。” 旁边一人拿过 GPS,  蒙上眼，居然沿着小径走到了树下。“这么厉害！你是什么人?”“卡尔曼 ! ”“卡尔曼？！你是卡尔曼？”众人大吃一惊。“我是说这个 GPS 卡而慢。”卡尔曼滤波器与我以前讲过的FIR, IIR 滤波器完全不一样，与其说属于滤波器，不如说是属于最优控制的范畴。下面的内容涉及相当多的控制理论知识，对于在这方面不足的同学可能有些吃力。不过不要紧，大家关注结果，会应用就够了,  那些晦涩的理论和推导可以忽略。我也会用图片让大家更直观的理解卡尔曼滤波器。首先回顾一下传统数字滤波器。对于一个线性时不变系统，施加一个输入 u(t) ，我们可以得到一个输出 y(t) . 如果输入是一个冲击，则输出y(t) 被称作冲击响应，用h(t) 来表示，是系统的内核。对于任意 u(t), 输出 y(t) 可以通过 u(t) 与冲击响应 h(t) 的卷积得到，这是 FIR 滤波器的基本原理。我们还可以通过系统微分方程转换为差分方程，或是通过 laplace 传递函数转换到差分方程，最后得到一个递推公式，这种形式的滤波器就是IIR 滤波器。以前讲过，一个系统可以用时域的微分方程来建立，然后可以用 laplace 的传递函数来处理，把解微分方程变为多项式乘法，可以简单的求解。还有另外一种处理形式就是状态空间，以矩阵形式来处理微分方程或微分方程组，利用矩阵变换求解，类同齐次方程组的矩阵形式。例如微分方程:              y’’ +3y’ + 2y =u让      X1= y,       X2 = y’ = X1’,   则上式变为：             X2’ = -3 X2  – 2 X1 - u             X1’ =      X2矩阵形式为：
通用形式为：           X’ = A*X + B*u           Y = C*X.可以看到，可以很轻易的微分方程或微分方程组转换到状态空间形式，而状态空间与laplace 传递函数之间可以相互转换，事实上矩阵A 的特征值就是s传递函数的极点。系统的传递函数（阵）可以通过矩阵变换得到：               Y(s) = C * (s *I - A) -1 * B同理，连续域的微分方程对应了离散域的差分方程，s 对应了z,   离散域状态空间相应的变为：              X(k) = A*X(k-1)+ B(u-1)              Y(k) = C*X(k)我们现在来看看蒙眼走小径的走法问题。假设A 走过的路径是真真正的路径，为Za; B是用自己的大脑作为预测估计器，走出了一个预测路径，为 Zb; C 用测量器，走出了一条测量路径，为Zc。用图片来说明：
“系统真实输出”是 A 走过的路径： Za = C * X；“测量输出”是Zb.   Zb = Za + V，这里 V 是噪声，即GPS 的漂移；“预测估计输出”是 Zc = C * X^,X^是预测的状态。T 是采样延时。现在，蒙上眼的情况下有两种选择，GPS 或大脑预测估计器。如果GPS很准而预测不准，那么可以选择GPS；如果预测准确而GPS不准，那么选择预测估计器，等等， 没有反馈的预测估计器会因为累积误差而导致越来越不准。如果两个都不准，该如何取舍？如何把两者结合在一起呢？ 我们可以设置一个信心指数 K，K 在 0 与1之间，来说明对测量值还是预测值的信任程度： Z = K * Zb + (1 – K) * Zc  = Zc + K*(Zb – Zc)          (1)可以看出， 当 K = 1 和 0 时，分别选择了GPS 或预测估计器. 现在，可以把误差 Zb -Zc 作为反馈误差，来修正 预测估计器的结果。新的系统结构图如下：
这个框图，就是卡尔曼滤波器的基本构造。学过现代控制理论的同学都这个图应该很熟悉，与状态变量估计控制的图形差不多，只是其中的 K = 1 而且没有噪声项和系统反馈而已。而我们下面的任务，就是如何确定这个K 值。......以下略去三百字的方差，与协方差的介绍.  自己看吧： http://zh.wikipedia.org/wiki/%E5%8D%8F%E6%96%B9%E5%B7%AE........以下略去 五百字的kalman Filter Gain K 的推导。自己看吧： http://zh.wikipedia.org/wiki/%E5%8D%A1%E5%B0%94%E6%9B%BC%E6%BB%A4%E6%B3%A2关于卡尔曼滤波器的推导过程，枯燥晦涩，我就略过，直接关注结果。计算过程：卡尔曼滤波是一种递归的估计，即只要获知上一时刻状态的估计值以及当前状态的观测值就可以计算出当前状态的估计值，因此不需要记录观测或者估计的历史信息。卡尔曼滤波器的递归过程。1）估计时刻k 的状态：X(k) = A*X(k-1) + B*u(k)这里,   u(k) 是系统输入。2 ) 计算误差相关矩阵P, 度量估计值的精确程度：P(k) = A*P(k-1)*A’+ Q这里,   Q = E{ Wj^2 } 是系统噪声的协方差阵，即系统框图中的Wj的协方差阵, Q 应该是不断变化的，为了简化，当作一个常数矩阵。3 ) 计算卡尔曼增益, 以下略去 (k),  即 P = P(k), X = X(k)：K = P *C’ * (C * P * C’ + R) -1 这里 R = E{ Vj^2 }, 是测量噪声的协方差(阵),  即系统框图中的 Vj 的协方差, 为了简化，也当作一个常数矩阵。由于我们的系统一般是单输入单输出，所以 R是一个 1x1的矩阵，即一个常数，上面的公式可以简化为:K = P *C’ / (C * P * C’ + R) 4 ) 状态变量反馈的误差量：e = Z(k) – C*X(k)这里的 Z(k) 是带噪声的测量量5 ) 更新误差相关矩阵PP = P – K * C * P6 ) 更新状态变量： X =X + K*e = X + K* (Z(k) – C*X(k))7 ) 最后的输出： Y = C*X现在的问题就是如何实现卡尔曼滤波， A, B, C, Q, R 这些矩阵或量如何确定？仿真实例下面用仿真实例来观察卡尔曼滤波器的效果。假设我们的系统是一个加热系统，热时间常数为 60 秒，100度时达到热平衡。忽略系统的延迟，那么当系统加电后，温度由 0开始上升。这个上升过程大家应该很熟悉，这是一个指数函数：y(t) = 100 * (1 – e-t/60)其 laplace 传递函数为: y(s) = 100 / (60 * s - 1)我们人为的加入了随机噪声来模拟测量噪声
我们假定并不知道系统的传递函数，现在只是简单的, 随便地构造了一个预测系统。A = [1, 0; 0, 1]B = [1; 0]C = [1, 0]这是一个二阶系统，其输出是一条直线，与实际的系统相去甚远：
测量噪声的协方差 R = 40，此为猜测值; 系统噪声 Q = 2，也是猜测值，预测模型越不准，Q 值应越大。卡尔曼滤波器的结果，红色为滤波器输出：
可以看到，尽管我们使用了一个粗劣的预测估计器，Kalman 滤波器还是相当的皮实，基本上消除了噪声.。如果我们有一个相当精确的模型，结果会怎么样呢？精确模型的建立要建立一个精确的预测估计模型，我们还是要利用方差。如果一个估计的曲线与实际曲线完全重合时，他们的方差为 0. 方差越小，拟合度越高， 最小二乘法的原理便是如此。具体推导过程还是省略，直接给出 matlab 的拟合程序，这是一个非常非常有用的程序。如果数学模型很精确， 能不能直接数学模型的输出作为滤波器的结果呢？不能，因为没有反馈，数学模型的输出会因为没有反馈的校正造成误差不断累积，失之毫厘，谬之千里。下面是用最小二乘法获得系统的模型并做为预测估计器，设定为 3 阶系统， 得出的数学模型相当准确，所以Q值可以取一个小值，这里 Q= 0.02, 现在看看卡尔曼滤波器的结果：
效果非常好，卡尔曼滤波器的输出与实际系统的输出 (即无噪声的系统输出) 几乎重合，这是精确的预测估计模型带来的好处。现在比较两个例子中 卡尔曼增益的不同 。

最小二乘法获得系统的模型中的增益迅速地由大变小，最后小于不准确模型。K 值较小，意味着误差反馈量较小， 使得预测输出更偏重信任预测模型的结果，通过上图可以看到 kalman 算法的自适应性。至于误差相关阵 P 的值，同样，最小二乘法获得系统的模型中的 C*P*C'的值较小， 这里就不给出图形了。结论：从上面的例子可以看出，卡尔曼滤波器对于预测系统的要求并不高，所以，那个”卡尔曼”可以蒙上眼，拿着一个劣质的 GPS 可以走到树下。若系统的预测模型很准确，卡尔曼滤波器会有一个相当良好的效果。matlab 的验证程序z在 62， 63楼，所有程序均为原创。第一个例子中的代码：Ts = 1;                    采样时间s = tf('s');sysc = 100/(60*s +1);      真实系统的传递函数sysd = c2d(sysc, Ts); t = 0:TsTs*300);u = ones(1, length(t));     系统输入ys = lsim(sysd, u, t, 0);   系统输出ys = ys + 10*rand(size(ys));  测量量 = 系统输出 + 噪声 A = [1, 0; 0, 1];B = [1; 0];C = [1, 0];N = length(A);          系统维数Len = length(u); yout = zeros(Len, 1);   滤波器输出Xh = zeros(N, 1);       状态变量P = eye(N);             Q = 2 * eye(N);         系统噪声R = 50;                 测量噪声for i = 1 : Len   Xh = A*Xh + B*u(i);    P= A*P*A' + Q;    K= P*C'* inv(C*P*C' + R);   Xh = Xh + K*(ys(i) - C*Xh);    P= P - K*C*P;   yout(i) = C*Xh;endTs = 1;                    采样时间s = tf('s');sysc = 100/(60*s +1);      真实系统的传递函数sysd = c2d(sysc, Ts); t = 0:Ts:(Ts*300);u = ones(1, length(t));     系统输入ys = lsim(sysd, u, t, 0);   系统输出ys = ys + 10*rand(size(ys));  测量量 = 系统输出 + 噪声 A = [1, 0; 0, 1];B = [1; 0];C = [1, 0];N = length(A);          系统维数Len = length(u); yout = zeros(Len, 1);   滤波器输出Xh = zeros(N, 1);       状态变量P = eye(N);             Q = 2 * eye(N);         系统噪声R = 50;                 测量噪声for i = 1 : Len   Xh = A*Xh + B*u(i);    P= A*P*A' + Q;    K= P*C'* inv(C*P*C' + R);   Xh = Xh + K*(ys(i) - C*Xh);    P= P - K*C*P;   yout(i) = C*Xh;end plot(t, ys, t, yout);使用最小二乘法例子的代码：   Ts = 1;               采样时间s = tf('s');sysc = 100/(60*s +1);   真实系统的传递函数sysd = c2d(sysc, Ts); t = 0:Ts:(Ts*300);u = ones(1, length(t));   系统输入ys = lsim(sysd, u, t, 0); 系统输出ys = ys + 10*rand(size(ys)); 测量量 = 系统输出 + 噪声 [numd, dend] = LeastSquare(u, ys, 3);   最小二乘法获取预测系统模型[A, B, C, D] = tf2ss(numd, dend);       变换到状态空间形式Len = length(u);N = length(A);         系统维数 yout = zeros(Len, 1);  滤波器输出Xh = zeros(N, 1);      状态变量P = eye(N);Q = 0.02 * eye(N);     系统噪声R = 50;                测量噪声for i = 1 : Len   Xh = A*Xh + B*u(i);    P= A*P*A' + Q;     K= P*C'* inv(C*P*C' + R);   Xh = Xh + K*(ys(i) - C*Xh);    P= P - K*C*P;   yout(i) = C*Xh;end plot(t, ys, t, yout);最小二乘法获取系统传递函数：function [numd, dend] = LeastSquare(x, y,N)count = length(y);M = count - 1;ai = zeros(N*2, M);for i=1:N   ai(i, i:M) = x(1:(count-i));   ai(i+N, i:M) = -y(1:(count-i));endbi = y(2:(count));xd = (inv(ai*ai')*ai*bi)';numd = [0 xd(1:N)];dend = [1 xd(N+1: N+N)]; 输入参数：x  ---  系统输入阵列y  ---  系统输出阵列N  ---  系统传递函数的阶数函数输出：系统 Z 传递函数分子与分母的系数，即： h(z) = numd(z) / dend(z)这个函数实在是居家旅行 ..., 啊，是建模仿真必备良器。

7leaves

一片绿油油的草地上有一条曲折的小径，通向一棵大树。一个要求被提出：从起点沿着小径走到树下。

“很简单。” A说，于是他丝毫不差地沿着小径走到了树下。

现在，难度被增加了：蒙上眼。

“也不难，我当过特种兵。” B说，于是他歪歪扭扭地走到了树………. 旁。“唉，好久不练，生疏了。”

“看我的，我有 DIY 的 GPS！” C说，于是他像个醉汉似地走到了树………. 旁。“唉，这个 GPS 软件没做好，漂移太大。”

“我来试试。” 旁边一人拿过 GPS,  蒙上眼，居然沿着小径走到了树下。

“这么厉害！你是什么人?”

“卡尔曼 ! ”

“卡尔曼？！你是卡尔曼？”众人大吃一惊。

“我是说这个 GPS 卡而慢。”

卡尔曼滤波器与我以前讲过的FIR, IIR 滤波器完全不一样，与其说属于滤波器，不如说是属于最优控制的范畴。下面的内容涉及相当多的控制理论知识，对于在这方面不足的同学可能有些吃力。不过不要紧，大家关注结果，会应用就够了,  那些晦涩的理论和推导可以忽略。我也会用图片让大家更直观的理解卡尔曼滤波器。

首先回顾一下传统数字滤波器。

对于一个线性时不变系统，施加一个输入 u(t) ，我们可以得到一个输出 y(t) . 如果输入是一个冲击，则输出y(t) 被称作冲击响应，用h(t) 来表示，是系统的内核。对于任意 u(t), 输出 y(t) 可以通过 u(t) 与冲击响应 h(t) 的卷积得到，这是 FIR 滤波器的基本原理。我们还可以通过系统微分方程转换为差分方程，或是通过 laplace 传递函数转换到差分方程，最后得到一个递推公式，这种形式的滤波器就是IIR 滤波器。

以前讲过，一个系统可以用时域的微分方程来建立，然后可以用 laplace 的传递函数来处理，把解微分方程变为多项式乘法，可以简单的求解。还有另外一种处理形式就是状态空间，以矩阵形式来处理微分方程或微分方程组，利用矩阵变换求解，类同齐次方程组的矩阵形式。例如微分方程:

              y’’ +3y’ + 2y =u

让      X1= y,       X2 = y’ = X1’,   则上式变为：

             X2’ = -3 X2  – 2 X1 - u

             X1’ =      X2

矩阵形式为：

通用形式为：

           X’ = A*X + B*u

           Y = C*X.

可以看到，可以很轻易的微分方程或微分方程组转换到状态空间形式，而状态空间与laplace 传递函数之间可以相互转换，事实上

矩阵A 的特征值就是s传递函数的极点。系统的传递函数（阵）可以通过矩阵变换得到：

               Y(s) = C * (s *I - A) -1 * B

同理，连续域的微分方程对应了离散域的差分方程，s 对应了z,   离散域状态空间相应的变为：

              X(k) = A*X(k-1)+ B(u-1)

              Y(k) = C*X(k)

我们现在来看看蒙眼走小径的走法问题。

假设A 走过的路径是真真正的路径，为Za; B是用自己的大脑作为预测估计器，走出了一个预测路径，为 Zb; C 用测量器，走出了一条测量路径，为Zc。用图片来说明：

“系统真实输出”是 A 走过的路径： Za = C * X；

“测量输出”是Zb.   Zb = Za + V，

这里 V 是噪声，即GPS 的漂移；

“预测估计输出”是 Zc = C * X^,

X^是预测的状态。

T 是采样延时。

现在，蒙上眼的情况下有两种选择，GPS 或大脑预测估计器。如果GPS很准而预测不准，那么可以选择GPS；

如果预测准确而GPS不准，那么选择预测估计器，等等， 没有反馈的预测估计器会因为累积误差而导致越来越不准。如果两个都不准，该如何取舍？如何把两者结合在一起呢？

我们可以设置一个信心指数 K，K 在 0 与1之间，来说明对测量值还是预测值的信任程度：

Z = K * Zb + (1 – K) * Zc  = Zc + K*(Zb – Zc)          (1)

可以看出， 当 K = 1 和 0 时，分别选择了GPS 或预测估计器. 现在，可以把误差 Zb -Zc 作为反馈误差，来修正 预测估计器的结果。

新的系统结构图如下：

这个框图，就是卡尔曼滤波器的基本构造。学过现代控制理论的同学都这个图应该很熟悉，与状态变量估计控制的图形差不多，只是其中的 K = 1 而且没有噪声项和系统反馈而已。

而我们下面的任务，就是如何确定这个K 值。

......

以下略去

关于卡尔曼滤波器的推导过程，枯燥晦涩，我就略过，直接关注结果。

计算过程：

卡尔曼滤波是一种递归的估计，即只要获知上一时刻状态的估计值以及当前状态的观测值就可以计算出当前状态的估计值，因此不需要记录观测或者估计的历史信息。卡尔曼滤波器的递归过程。

1）估计时刻k 的状态：

X(k) = A*X(k-1) + B*u(k)

这里,   u(k) 是系统输入。

2 ) 计算误差相关矩阵P, 度量估计值的精确程度：

P(k) = A*P(k-1)*A’+ Q

这里,   Q = E{ Wj^2 } 是系统噪声的协方差阵，即系统框图中的Wj的协方差阵, Q 应该是不断变化的，为了简化，当作一个常数矩阵。

3 ) 计算卡尔曼增益, 以下略去 (k),  即 P = P(k), X = X(k)：

K = P *C’ * (C * P * C’ + R) -1

这里 R = E{ Vj^2 }, 是测量噪声的协方差(阵),  即系统框图中的 Vj 的协方差, 为了简化，也当作一个常数矩阵。由于我们的系统一般是单输入单输出，所以 R是一个 1x1的矩阵，即一个常数，上面的公式可以简化为:

K = P *C’ / (C * P * C’ + R)

4 ) 状态变量反馈的误差量：

e = Z(k) – C*X(k)

这里的 Z(k) 是带噪声的测量量

5 ) 更新误差相关矩阵P

P = P – K * C * P

6 ) 更新状态变量：

X =X + K*e = X + K* (Z(k) – C*X(k))

7 ) 最后的输出：

Y = C*X

现在的问题就是如何实现卡尔曼滤波， A, B, C, Q, R 这些矩阵或量如何确定？

仿真实例

下面用仿真实例来观察卡尔曼滤波器的效果。假设我们的系统是一个加热系统，热时间常数为 60 秒，100度时达到热平衡。忽略系统的延迟，那么当系统加电后，温度由 0开始上升。这个上升过程大家应该很熟悉，这是一个指数函数：

y(t) = 100 * (1 – e-t/60)

其 laplace 传递函数为:

y(s) = 100 / (60 * s - 1)

我们人为的加入了随机噪声来模拟测量噪声

我们假定并不知道系统的传递函数，现在只是简单的, 随便地构造了一个预测系统。

A = [1, 0; 0, 1]

B = [1; 0]

C = [1, 0]

这是一个二阶系统，其输出是一条直线，与实际的系统相去甚远：

测量噪声的协方差 R = 40，此为猜测值; 系统噪声 Q = 2，也是猜测值，预测模型越不准，Q 值应越大。

卡尔曼滤波器的结果，红色为滤波器输出：

可以看到，尽管我们使用了一个粗劣的预测估计器，Kalman 滤波器还是相当的皮实，基本上消除了噪声.。

如果我们有一个相当精确的模型，结果会怎么样呢？

精确模型的建立

要建立一个精确的预测估计模型，我们还是要利用方差。如果一个估计的曲线与实际曲线完全重合时，他们的方差为 0. 方差越小，拟合度越高， 最小二乘法的原理便是如此。具体推导过程还是省略，直接给出 matlab 的拟合程序，这是一个非常非常有用的程序。

如果数学模型很精确， 能不能直接数学模型的输出作为滤波器的结果呢？不能，因为没有反馈，数学模型的输出会因为没有反馈的校正造成误差不断累积，失之毫厘，谬之千里。

下面是用最小二乘法获得系统的模型并做为预测估计器，设定为 3 阶系统， 得出的数学模型相当准确，所以Q值可以取一个小值，

这里 Q= 0.02, 现在看看卡尔曼滤波器的结果：

效果非常好，卡尔曼滤波器的输出与实际系统的输出 (即无噪声的系统输出) 几乎重合，这是精确的预测估计模型带来的好处。

现在比较两个例子中 卡尔曼增益的不同 。

最小二乘法获得系统的模型中的增益迅速地由大变小，最后小于不准确模型。K 值较小，意味着误差反馈量较小， 使得预测输出更偏重信任预测模型的结果，通过上图可以看到 kalman 算法的自适应性。至于误差相关阵 P 的值，同样，最小二乘法获得系统的模型中的 C*P*C'的值较小， 这里就不给出图形了。

结论：

从上面的例子可以看出，卡尔曼滤波器对于预测系统的要求并不高，所以，那个”卡尔曼”可以蒙上眼，拿着一个劣质的 GPS 可以走到树下。若系统的预测模型很准确，卡尔曼滤波器会有一个相当良好的效果。

matlab 的验证程序z在 62， 63楼，所有程序均为原创。

第一个例子中的代码：

Ts = 1;                    采样时间

s = tf('s');

sysc = 100/(60*s +1);      真实系统的传递函数

sysd = c2d(sysc, Ts);

t = 0:TsTs*300);

u = ones(1, length(t));     系统输入

ys = lsim(sysd, u, t, 0);   系统输出

ys = ys + 10*rand(size(ys));  测量量 = 系统输出 + 噪声

A = [1, 0; 0, 1];

B = [1; 0];

C = [1, 0];

N = length(A);          系统维数

Len = length(u);

yout = zeros(Len, 1);   滤波器输出

Xh = zeros(N, 1);       状态变量

P = eye(N);            

Q = 2 * eye(N);         系统噪声

R = 50;                 测量噪声

for i = 1 : Len

   Xh = A*Xh + B*u(i);

    P= A*P*A' + Q;

    K= P*C'* inv(C*P*C' + R);

   Xh = Xh + K*(ys(i) - C*Xh);

    P= P - K*C*P;

   yout(i) = C*Xh;

end

Ts = 1;                    采样时间

s = tf('s');

sysc = 100/(60*s +1);      真实系统的传递函数

sysd = c2d(sysc, Ts);

t = 0:Ts:(Ts*300);

u = ones(1, length(t));     系统输入

ys = lsim(sysd, u, t, 0);   系统输出

ys = ys + 10*rand(size(ys));  测量量 = 系统输出 + 噪声

A = [1, 0; 0, 1];

B = [1; 0];

C = [1, 0];

N = length(A);          系统维数

Len = length(u);

yout = zeros(Len, 1);   滤波器输出

Xh = zeros(N, 1);       状态变量

P = eye(N);            

Q = 2 * eye(N);         系统噪声

R = 50;                 测量噪声

for i = 1 : Len

   Xh = A*Xh + B*u(i);

    P= A*P*A' + Q;

    K= P*C'* inv(C*P*C' + R);

   Xh = Xh + K*(ys(i) - C*Xh);

    P= P - K*C*P;

   yout(i) = C*Xh;

end

plot(t, ys, t, yout);

使用最小二乘法例子的代码：

Ts = 1;               采样时间

s = tf('s');

sysc = 100/(60*s +1);   真实系统的传递函数

sysd = c2d(sysc, Ts);

t = 0:Ts:(Ts*300);

u = ones(1, length(t));   系统输入

ys = lsim(sysd, u, t, 0); 系统输出

ys = ys + 10*rand(size(ys)); 测量量 = 系统输出 + 噪声

[numd, dend] = LeastSquare(u, ys, 3);   最小二乘法获取预测系统模型

[A, B, C, D] = tf2ss(numd, dend);       变换到状态空间形式

Len = length(u);

N = length(A);         系统维数

yout = zeros(Len, 1);  滤波器输出

Xh = zeros(N, 1);      状态变量

P = eye(N);

Q = 0.02 * eye(N);     系统噪声

R = 50;                测量噪声

for i = 1 : Len

   Xh = A*Xh + B*u(i);

    P= A*P*A' + Q;

    K= P*C'* inv(C*P*C' + R);

   Xh = Xh + K*(ys(i) - C*Xh);

    P= P - K*C*P;

   yout(i) = C*Xh;

end

plot(t, ys, t, yout);

最小二乘法获取系统传递函数：

function [numd, dend] = LeastSquare(x, y,N)

count = length(y);

M = count - 1;

ai = zeros(N*2, M);

for i=1:N

   ai(i, i:M) = x(1:(count-i));

   ai(i+N, i:M) = -y(1:(count-i));

end

bi = y(2:(count));

xd = (inv(ai*ai')*ai*bi)';

numd = [0 xd(1:N)];

dend = [1 xd(N+1: N+N)];

输入参数：

x  ---  系统输入阵列

y  ---  系统输出阵列

N  ---  系统传递函数的阶数

函数输出：

系统 Z 传递函数分子与分母的系数，即： h(z) = numd(z) / dend(z)

这个函数实在是居家旅行 ..., 啊，是建模仿真必备良器。

maylove

图片打不开，你复制后点一下下载远程图片。

7leaves

不让编辑了？什么原因

郝胖德

虽然没看完呢，但觉得写得挺好的

considering

好厉害呀，可惜我完全看不懂，好羞愧呀

azhiking

7leaves 发表于 2012-12-28 22:11
一片绿油油的草地上有一条曲折的小径，通向一棵大树。一个要求被提出：从起点沿着小径走到树下。“很简单。” A说，于是他丝毫不差地沿着小径走到了树下。现在，难度被增加了：蒙上眼。“也不难，我当过特种兵。” B说，于是他歪歪扭扭地走到了树………. 旁。“唉，好久不练，生疏了。”“看我的，我有 DIY 的 GPS！” C说，于是他像个醉汉似地走到了树………. 旁。“唉，这个 GPS 软件没做好，漂移太大。”“我来试试。” 旁边一人拿过 GPS,  蒙上眼，居然沿着小径走到了树下。“这么厉害！你是什么人?”“卡尔曼 ! ”“卡尔曼？！你是卡尔曼？”众人大吃一惊。“我是说这个 GPS 卡而慢。”卡尔曼滤波器与我以前讲过的FIR, IIR 滤波器完全不一样，与其说属于滤波器，不如说是属于最优控制的范畴。下面的内容涉及相当多的控制理论知识，对于在这方面不足的同学可能有些吃力。不过不要紧，大家关注结果，会应用就够了,  那些晦涩的理论和推导可以忽略。我也会用图片让大家更直观的理解卡尔曼滤波器。首先回顾一下传统数字滤波器。对于一个线性时不变系统，施加一个输入 u(t) ，我们可以得到一个输出 y(t) . 如果输入是一个冲击，则输出y(t) 被称作冲击响应，用h(t) 来表示，是系统的内核。对于任意 u(t), 输出 y(t) 可以通过 u(t) 与冲击响应 h(t) 的卷积得到，这是 FIR 滤波器的基本原理。我们还可以通过系统微分方程转换为差分方程，或是通过 laplace 传递函数转换到差分方程，最后得到一个递推公式，这种形式的滤波器就是IIR 滤波器。以前讲过，一个系统可以用时域的微分方程来建立，然后可以用 laplace 的传递函数来处理，把解微分方程变为多项式乘法，可以简单的求解。还有另外一种处理形式就是状态空间，以矩阵形式来处理微分方程或微分方程组，利用矩阵变换求解，类同齐次方程组的矩阵形式。例如微分方程:              y’’ +3y’ + 2y =u让      X1= y,       X2 = y’ = X1’,   则上式变为：             X2’ = -3 X2  – 2 X1 - u             X1’ =      X2矩阵形式为：
通用形式为：           X’ = A*X + B*u           Y = C*X.可以看到，可以很轻易的微分方程或微分方程组转换到状态空间形式，而状态空间与laplace 传递函数之间可以相互转换，事实上矩阵A 的特征值就是s传递函数的极点。系统的传递函数（阵）可以通过矩阵变换得到：               Y(s) = C * (s *I - A) -1 * B同理，连续域的微分方程对应了离散域的差分方程，s 对应了z,   离散域状态空间相应的变为：              X(k) = A*X(k-1)+ B(u-1)              Y(k) = C*X(k)我们现在来看看蒙眼走小径的走法问题。假设A 走过的路径是真真正的路径，为Za; B是用自己的大脑作为预测估计器，走出了一个预测路径，为 Zb; C 用测量器，走出了一条测量路径，为Zc。用图片来说明：  

“系统真实输出”是 A 走过的路径： Za = C * X；“测量输出”是Zb.   Zb = Za + V，这里 V 是噪声，即GPS 的漂移；“预测估计输出”是 Zc = C * X^,X^是预测的状态。T 是采样延时。现在，蒙上眼的情况下有两种选择，GPS 或大脑预测估计器。如果GPS很准而预测不准，那么可以选择GPS；如果预测准确而GPS不准，那么选择预测估计器，等等， 没有反馈的预测估计器会因为累积误差而导致越来越不准。如果两个都不准，该如何取舍？如何把两者结合在一起呢？ 我们可以设置一个信心指数 K，K 在 0 与1之间，来说明对测量值还是预测值的信任程度： Z = K * Zb + (1 – K) * Zc  = Zc + K*(Zb – Zc)          (1)可以看出， 当 K = 1 和 0 时，分别选择了GPS 或预测估计器. 现在，可以把误差 Zb -Zc 作为反馈误差，来修正 预测估计器的结果。新的系统结构图如下：


这个框图，就是卡尔曼滤波器的基本构造。学过现代控制理论的同学都这个图应该很熟悉，与状态变量估计控制的图形差不多，只是其中的 K = 1 而且没有噪声项和系统反馈而已。而我们下面的任务，就是如何确定这个K 值。......以下略去关于卡尔曼滤波器的推导过程，枯燥晦涩，我就略过，直接关注结果。计算过程：卡尔曼滤波是一种递归的估计，即只要获知上一时刻状态的估计值以及当前状态的观测值就可以计算出当前状态的估计值，因此不需要记录观测或者估计的历史信息。卡尔曼滤波器的递归过程。1）估计时刻k 的状态：X(k) = A*X(k-1) + B*u(k)这里,   u(k) 是系统输入。2 ) 计算误差相关矩阵P, 度量估计值的精确程度：P(k) = A*P(k-1)*A’+ Q这里,   Q = E{ Wj^2 } 是系统噪声的协方差阵，即系统框图中的Wj的协方差阵, Q 应该是不断变化的，为了简化，当作一个常数矩阵。3 ) 计算卡尔曼增益, 以下略去 (k),  即 P = P(k), X = X(k)：K = P *C’ * (C * P * C’ + R) -1 这里 R = E{ Vj^2 }, 是测量噪声的协方差(阵),  即系统框图中的 Vj 的协方差, 为了简化，也当作一个常数矩阵。由于我们的系统一般是单输入单输出，所以 R是一个 1x1的矩阵，即一个常数，上面的公式可以简化为:K = P *C’ / (C * P * C’ + R) 4 ) 状态变量反馈的误差量：e = Z(k) – C*X(k)这里的 Z(k) 是带噪声的测量量5 ) 更新误差相关矩阵PP = P – K * C * P6 ) 更新状态变量： X =X + K*e = X + K* (Z(k) – C*X(k))7 ) 最后的输出： Y = C*X现在的问题就是如何实现卡尔曼滤波， A, B, C, Q, R 这些矩阵或量如何确定？仿真实例下面用仿真实例来观察卡尔曼滤波器的效果。假设我们的系统是一个加热系统，热时间常数为 60 秒，100度时达到热平衡。忽略系统的延迟，那么当系统加电后，温度由 0开始上升。这个上升过程大家应该很熟悉，这是一个指数函数：y(t) = 100 * (1 – e-t/60)其 laplace 传递函数为: y(s) = 100 / (60 * s - 1)我们人为的加入了随机噪声来模拟测量噪声


我们假定并不知道系统的传递函数，现在只是简单的, 随便地构造了一个预测系统。A = [1, 0; 0, 1]B = [1; 0]C = [1, 0]这是一个二阶系统，其输出是一条直线，与实际的系统相去甚远：

测量噪声的协方差 R = 40，此为猜测值; 系统噪声 Q = 2，也是猜测值，预测模型越不准，Q 值应越大。卡尔曼滤波器的结果，红色为滤波器输出：


可以看到，尽管我们使用了一个粗劣的预测估计器，Kalman 滤波器还是相当的皮实，基本上消除了噪声.。如果我们有一个相当精确的模型，结果会怎么样呢？精确模型的建立要建立一个精确的预测估计模型，我们还是要利用方差。如果一个估计的曲线与实际曲线完全重合时，他们的方差为 0. 方差越小，拟合度越高， 最小二乘法的原理便是如此。具体推导过程还是省略，直接给出 matlab 的拟合程序，这是一个非常非常有用的程序。如果数学模型很精确， 能不能直接数学模型的输出作为滤波器的结果呢？不能，因为没有反馈，数学模型的输出会因为没有反馈的校正造成误差不断累积，失之毫厘，谬之千里。下面是用最小二乘法获得系统的模型并做为预测估计器，设定为 3 阶系统， 得出的数学模型相当准确，所以Q值可以取一个小值，这里 Q= 0.02, 现在看看卡尔曼滤波器的结果：


效果非常好，卡尔曼滤波器的输出与实际系统的输出 (即无噪声的系统输出) 几乎重合，这是精确的预测估计模型带来的好处。现在比较两个例子中 卡尔曼增益的不同 。


最小二乘法获得系统的模型中的增益迅速地由大变小，最后小于不准确模型。K 值较小，意味着误差反馈量较小， 使得预测输出更偏重信任预测模型的结果，通过上图可以看到 kalman 算法的自适应性。至于误差相关阵 P 的值，同样，最小二乘法获得系统的模型中的 C*P*C'的值较小， 这里就不给出图形了。
结论：从上面的例子可以看出，卡尔曼滤波器对于预测系统的要求并不高，所以，那个”卡尔曼”可以蒙上眼，拿着一个劣质的 GPS 可以走到树下。若系统的预测模型很准确，卡尔曼滤波器会有一个相当良好的效果。matlab 的验证程序z在 62， 63楼，所有程序均为原创。第一个例子中的代码：Ts = 1;                    采样时间s = tf('s');sysc = 100/(60*s +1);      真实系统的传递函数sysd = c2d(sysc, Ts);
t = 0:TsTs*300);u = ones(1, length(t));     系统输入ys = lsim(sysd, u, t, 0);   系统输出ys = ys + 10*rand(size(ys));  测量量 = 系统输出 + 噪声
A = [1, 0; 0, 1];B = [1; 0];C = [1, 0];N = length(A);          系统维数Len = length(u);
yout = zeros(Len, 1);   滤波器输出Xh = zeros(N, 1);       状态变量P = eye(N);             Q = 2 * eye(N);         系统噪声R = 50;                 测量噪声for i = 1 : Len   Xh = A*Xh + B*u(i);    P= A*P*A' + Q;    K= P*C'* inv(C*P*C' + R);   Xh = Xh + K*(ys(i) - C*Xh);    P= P - K*C*P;   yout(i) = C*Xh;endTs = 1;                    采样时间s = tf('s');sysc = 100/(60*s +1);      真实系统的传递函数sysd = c2d(sysc, Ts);
t = 0:Ts:(Ts*300);u = ones(1, length(t));     系统输入ys = lsim(sysd, u, t, 0);   系统输出ys = ys + 10*rand(size(ys));  测量量 = 系统输出 + 噪声
A = [1, 0; 0, 1];B = [1; 0];C = [1, 0];N = length(A);          系统维数Len = length(u);
yout = zeros(Len, 1);   滤波器输出Xh = zeros(N, 1);       状态变量P = eye(N);             Q = 2 * eye(N);         系统噪声R = 50;                 测量噪声for i = 1 : Len   Xh = A*Xh + B*u(i);    P= A*P*A' + Q;    K= P*C'* inv(C*P*C' + R);   Xh = Xh + K*(ys(i) - C*Xh);    P= P - K*C*P;   yout(i) = C*Xh;end
plot(t, ys, t, yout);使用最小二乘法例子的代码：

Ts = 1;               采样时间s = tf('s');sysc = 100/(60*s +1);   真实系统的传递函数sysd = c2d(sysc, Ts);
t = 0:Ts:(Ts*300);u = ones(1, length(t));   系统输入ys = lsim(sysd, u, t, 0); 系统输出ys = ys + 10*rand(size(ys)); 测量量 = 系统输出 + 噪声
[numd, dend] = LeastSquare(u, ys, 3);   最小二乘法获取预测系统模型[A, B, C, D] = tf2ss(numd, dend);       变换到状态空间形式Len = length(u);N = length(A);         系统维数
yout = zeros(Len, 1);  滤波器输出Xh = zeros(N, 1);      状态变量P = eye(N);Q = 0.02 * eye(N);     系统噪声R = 50;                测量噪声for i = 1 : Len   Xh = A*Xh + B*u(i);    P= A*P*A' + Q;
    K= P*C'* inv(C*P*C' + R);   Xh = Xh + K*(ys(i) - C*Xh);    P= P - K*C*P;   yout(i) = C*Xh;end
plot(t, ys, t, yout);最小二乘法获取系统传递函数：function [numd, dend] = LeastSquare(x, y,N)count = length(y);M = count - 1;ai = zeros(N*2, M);for i=1:N   ai(i, i:M) = x(1:(count-i));   ai(i+N, i:M) = -y(1:(count-i));endbi = y(2:(count));xd = (inv(ai*ai')*ai*bi)';numd = [0 xd(1:N)];dend = [1 xd(N+1: N+N)];
输入参数：x  ---  系统输入阵列y  ---  系统输出阵列N  ---  系统传递函数的阶数函数输出：系统 Z 传递函数分子与分母的系数，即： h(z) = numd(z) / dend(z)这个函数实在是居家旅行 ..., 啊，是建模仿真必备良器。

谢谢分享，这个现在用的还比较多

hbthgs

谢谢分享

EdwardsThompson

谢谢，分享，LZ，涨姿势了

yutian_sl

好文！写得不错！

xiashuang

mark！学习学习