透过表象看本质！？之三——Kalman滤波透过表象看本质！？之三——Kalman滤波

现在的位置: 首页 > 综合 > 正文

透过表象看本质！？之三——Kalman滤波透过表象看本质！？之三——Kalman滤波

2017年11月03日 ⁄ 综合 ⁄ 共 6434字 ⁄ 字号小中大 ⁄ 评论关闭

透过表象看本质！？之三——Kalman滤波

分类：人工智能观测本质预测人工智能预测本质2013-08-29
01:01 681人阅读评论(8) 收藏举报

Kalman滤波预测数据分析

数据拟合能够估计出数据变化的趋势，另外一个同等重要的应用是如何利用这一趋势，预测下一时刻数据可能的值。通俗点儿说，你观察苍蝇（蚊子，蜜蜂）飞了几秒，你也许会想“它下一个时刻可能在哪儿”，“呈现出什么样的状态”诸如此类的问题。预知未来这档子事儿对我们有一种不可抗拒的吸引力。别看我们预测的未来很近，但这对于实际应用有很大的帮助。比如减小解空间的范围，便于搜索。对于搜索问题，预测可以看成是对从当前状态到目标状态的启发评价函数。好吧，我承认我陷得太深了，都是复习人工智能搞得。扯得有点儿远了，继续说我们的主题，预测。

古人每遇到重大活动，都会卜上一卦。念几句咒语，抽个签，看看签释，心里大概对所问之事有了个谱儿。再比如，这几天你的左眼皮一直在跳，你想知道这是为什么，意味着什么。你跑去算了一卦。抽签的时候，你心里默念着是不是要捡到钱了等等，结果抽了一个上上签，说你要遇到好事儿。“这几天眼皮跳”是你的观察数据。“你想知道未来会发生什么”是我们想要预测的东西。抽签的时候你心里默念的话，签儿，签上的符号和某些事件的对应关系，这些都是预测的算法。虽然占卜的过程包含了观察，有预测算法，有预测结果，同时也有结果的方差范围等等。但是我们说这种预测是不科学的，因为预测算法不科学，因果关系不见得成立等等。那有没有科学的预测呢，让我们进入今天的话题，Kalman滤波。

假设这样一个场景，A先生使用遥控器控制一架四轴直升飞机F在一个空旷的场地上飞行。直升飞机F上有一个GPS模块，通过无线发射模块实时的将直升飞机F的位置发给计算终端C。B先生在终端C上运行一个“打”直升飞机F的程序D。程序D根据终端C接收的GPS数据，指导一个虚拟的导弹E去跟踪直升飞机F，并试图将F“击落”。

A先生控制的直升飞机F飞行轨迹多变，很难被跟踪。同时，终端接收的GPS数据中还有噪声。B先生引导的导弹E燃料有限，因此不能长时间、频繁地机动。因此，B先生希望程序D要尽可能准的估计出F的位置，尽可能少机动，跟踪F并将其击落。

假如，A先生控制F急速攀升，如图1a。D得到的数据如图1b中的红色点。如果不进行预测，直接根据GPS数据控制E机动，E的运动轨迹如图1c，绿色的轨迹线。E很可能因燃料不足提前爆炸，而没有击中F。B先生很希望D能够根据GPS数据计算出如图1d所示的轨迹（橙色的轨迹线），来引导E去追踪F。

a b c d

图1 a、被观测对象实际的运动轨迹；b、我们观测到的被观测对象的运动轨迹；

c、如果不滤波的话，预测的轨迹；d、滤波后的预测轨迹。

假如A先生很狡猾，他控制F飞行，其飞行轨迹如图2所示。B先生深感压力巨大，如何才能有效的跟踪F，并将其击落呢。

图2 A先生控制的直升飞机F可能飞出的轨迹（终端C得到的GPS数据）。

颜色越深，获取数据的时间越早；反之，颜色越浅，获取时间越晚。

程序D根据终端C提供的GPS数据，估计F的位置（x，y），时间t的采样记为 ${s_t} = \left( {{x_t},{y_t}} \right)%$ 。使用前t个时刻的采样 ${s_1},{s_2},...,{s_t}%$ ，估计 $\hat f = g\left( {{s_1},{s_2},...,{s_t}} \right)%$ ，使得该估计满足，

$\min \sum\limits_{i = 1}^t {\left\| {\hat f\left( i \right) - {s_i}} \right\|} _p^p%$
(1)

$\min \left\| {\hat f\left( {i + 1} \right) - {s_{i + 1}}} \right\|_p^p%$
(2)

其中，为 ${\left\| \cdot \right\|_p}%$ 范数。公式(1)是对历史数据的平滑（smooth，filter），公式(2)是对未来数据的预测（predict）。求解 $\hat f = g\left( {{s_1},{s_2},...,{s_t}} \right)%$ 的过程是数据模型的更新。正如绪论中讨论的那样，数据模型可以形式化为，

$\hat f = g\left( {s,s',s'',...} \right)%$

其中s为观测直接得到的数据， $s'%$ 为观测数据的一阶微分或者偏微分， $s''%$ 为二阶微分或者偏微分，省略的部分为更高阶的微分或者偏微分。假如模型的复杂度函数h和模型涉及的数据的阶数相关，阶数越小复杂度越小，阶数越高复杂度越高。估计需要满足的第三个公式是

$\min h\left( {\hat f} \right)%$ (3)

模型的复杂度控制就是正则化。

B将D指导E跟踪F轨迹这一问题抽象为这样一个模型，其中涉及观察变量和状态变量。观察变量 $Z_{t}=\left [x\; y \right ]_{t}^{T}%$ 是终端C得到的GPS数据，状态变量 $X_{t}=\left [x\; y\; \dot{x}\; \dot{y} \right ]_{t}^{T}%$ 是程序D用于估计F位置的。Ft-1是状态转移矩阵，描述F的运动模型。Gt-1是控制矩阵， $U_{t-1}=\left [\ddot{x}\; \ddot{y} \right ]^{T}%$ 是外控制变量。Ht是观测矩阵，描述观测和状态之间的关系。Wt-1和Vt是高斯白噪声，covariance分别是Q和R，假设其不随状态变量变化。

${X_t} = {F_{t - 1}}{X_{t - 1}} + {G_{t - 1}}{U_{t - 1}} + {W_{t - 1}}%$
(4)

${Z_t} = {H_t}{X_t} + {V_t}%$ (5)

Ft和Ht的如何确定的呢？我们首先插入一段广告。对于一个具有n阶导数的函数f，其在x处的泰勒展开为

$f\left( x \right) = f\left( {{x_0}} \right) + \left( {x - {x_0}} \right)f'\left( {{x_0}} \right) + \frac{1}{2}{\left( {x - {x_0}} \right)^2}f''\left( {{x_0}} \right) + O\left( {{{\left( {x - {x_0}} \right)}^3}} \right)%$
(6)

忽略2阶以上的项，取x=t，x0=t-1，则上式可以写成

$f\left( t \right) \buildrel\textstyle.\over= f\left( {t - 1} \right) + f'\left( {t - 1} \right) + \frac{1}{2}f''\left( {t - 1} \right)%$
(7)

对上式分别求1阶和2阶导数有

$f'\left( t \right) \buildrel\textstyle.\over= f'\left( {t - 1} \right) + f''\left( {t - 1} \right)%$
(8)

$f''\left( t \right) \buildrel\textstyle.\over= f''\left( {t - 1} \right)%$
(9)

用矩阵的形式重写公式（7）如下

$\begin{pmatrix}f\\ f'\\ f''\\\end{pmatrix}_{t} \doteq \begin{pmatrix} 1& 1 & \frac{1}{2}\\ 0& 1 &1 \\ 0& 0 &1 \end{pmatrix}\begin{pmatrix}f\\ f'\\ f''\\\end{pmatrix}_{t-1}%$
(10)

对于离散模型的，微分用差分近似表示，式（8）（9）改写为

$f'\left( t \right) = f\left( t \right) - f\left( {t - 1} \right)%$
(11)

$f''\left( t \right) = f'\left( t \right) - f'\left( {t - 1} \right)\\ \;\;= \left( {f\left( t \right) - f\left( {t - 1} \right)} \right) - \left( {f\left( {t - 1} \right) - f\left( {t - 2} \right)} \right)\\ \;\;= f\left( t \right) - 2f\left( {t - 1} \right) + f\left( {t - 2} \right)%$
(12)

$\begin{pmatrix}f\\ f'\\ f''\\\end{pmatrix}_{t} \doteq \begin{pmatrix} 1& 0 &0 \\ 1& -1 &0 \\ 1& -2 &1 \end{pmatrix}\begin{pmatrix}f_{t}\\ f_{t-1}\\ f_{t-2}\\\end{pmatrix}_{t-1}%$
(13)

式（13）给出了要估计的函数、导数与观测数据之间的关系。

广告时间结束，言归正传Ft描述的是状态之间的关系。该关系受到运动学的基本关系式的约束。牛顿运动学定律可以使用式（10）表示。

如果，我们想实时更新状态变量的值，式（13）告诉我们，观测数据是如何影响状态变量的。如果不想实时更新，就可以仅用式（10）。

根据式（10），Ft和Ht的具体形式为

$F_{t}=\begin{pmatrix}1 &0 &1 &0 \\ 0 &1 &0 &1 \\ 0 &0 &1 &0 \\ 0 &0 &0 &1 \end{pmatrix}%$
(14)

$G_{t-1}=\begin{pmatrix}\frac{1}{2} &0 \\ 0 &\frac{1}{2} \\ 1 &0 \\ 0 &1 \end{pmatrix}%$
(15)

$H_{t}=\begin{pmatrix}1 &0 &0 &0 \\ 0 &1 &0 &0 \end{pmatrix}%$
(16)

在t时刻，根据式（4）预测F的当前位置 ${\hat X_t}%$ ，根据式（5）得到终端C得到的GPS数据的预测值 ${\hat Z_t}%$ 。使用t-1时刻的最优状态估计 $X\left( {\left. {t - 1} \right|t - 1} \right)%$ ，代入式（4）得

$\hat X\left( {\left. t \right|t - 1} \right) = {F_{t - 1}}\hat X\left( {\left. {t - 1} \right|t - 1} \right) + {G_{t - 1}}{U_{t - 1}}%$
（17）

$X\left( {\left. {t} \right|t - 1} \right)%$ 的covariance更新如下，其中covariance用P表示

$P\left( {\left. t \right|t - 1} \right) = {F_{t - 1}}P\left( {\left. {t - 1} \right|t - 1} \right){F_{t - 1}}^T + {Q_{t - 1}}%$
（18）

$P\left( {\left. {t} \right|t - 1} \right)%$ 是 $\hat X\left( {\left. {t} \right|t - 1} \right)%$ 的covariance， $P\left( {\left. {t - 1} \right|t - 1} \right)%$ 是 $\hat X\left( {\left. {t - 1} \right|t - 1} \right)%$ 的covariance。式（17）、（18）完成了预测，如何结合新的观测求解最优估计呢，继续往后看。t时刻的观测变量的预测

$\hat Z\left( {\left. t \right|t - 1} \right) = {H_t}\hat X\left( {\left. t \right|t - 1} \right)%$
（19）

观测变量的covariance

${S_t} = {H_t}P\left( {\left. t \right|t - 1} \right){H_t}^T + {R_t}%$
（20）

Kalman增益

${K_t} = P\left( {\left. t \right|t - 1} \right)H_t^TS_t^{ - 1}%$
（21）

t时刻观测变量的真实值与预测值之间的残差

${v_t} = {Z_t} - \hat Z\left( {\left. t \right|t - 1} \right)%$
（22）

t时刻观测变量的最优估计

$\hat X\left( {\left. t \right|t} \right) = \hat X\left( {\left. t \right|t - 1} \right) + {K_t} \cdot {v_t}%$
（23）

其covariance的最优估计是

$P\left( {\left. t \right|t} \right) = \left( {I - {K_t}{H_t}} \right)P\left( {\left. t \right|t - 1} \right)%$
（24）

公式（17）-（24）可以使用图3解释（图3使用观测变量来表示，而没有具体描述状态变量的预测和寻优的过程）。公式（17）-（19）对应的是图3b，根据图3a的t-1时刻的最优估计预测t时刻的观测变量。公式（20）-（24）对应的是图3d，根据图3c的新观测变量计算最优状态变量和观测变量。

$P\left( {\left. t \right|t - 1} \right) = E\left( {\left( {{X_t} - \hat X\left( {\left. t \right|t - 1} \right)} \right){{\left( {{X_t} - \hat X\left( {\left. t \right|t - 1} \right)} \right)}^T}} \right)%$
（25）

$P\left( {\left. t \right|t} \right) = E\left( {\left( {{X_t} - \hat X\left( {\left. t \right|t} \right)} \right){{\left( {{X_t} - \hat X\left( {\left. t \right|t} \right)} \right)}^T}} \right)%$
（26）

首先根据状态转移模型计算状态值的预测，求得观测变量的预测值。然后获得新的观测变量。再结合观测变量和观测变量的预测值，求出状态和观测变量的最优估计值。下面给出的是t时刻最优估计的模型，依然是高斯的。

$E\left( {{X_t}} \right) = \hat X\left( {\left. t \right|t} \right)%$

$E\left( {\left( {{X_t} - \hat X\left( {\left. t \right|t} \right)} \right){{\left( {{X_t} - \hat X\left( {\left. t \right|t} \right)} \right)}^T}} \right) = P\left( {\left. t \right|t} \right)%$

$p\left( {\left. {{X_t}} \right|{Z_t}} \right) \sim N\left( {\hat X\left( {\left. t \right|t} \right),P\left( {\left. t \right|t} \right)} \right)%$

a b c d

图3 F位置估计

Kalman Filter的matlab代码

[plain]view

 plaincopy

% the data to estimate  

lens=100;  

a=2;  

b=50;  

x=1:lens;  

y=a*x+b*randn(1,lens);  

D=[x;y];  

% the number of the stateparamters  

StateParamNum=4;  

% the number of thecontrol parameters  

ContrParamNum=2;  

% the number of theobservation parameters  

ObsevParamNum=2;  

% the motion transitionmatrix  

F=[1 0 1 0;0 1 0 1;0 0 1 0;0 00 1];  

% the control matrix  

G=[0.5 0;0 0.5;1 0;0 1];  

% the observation matrix  

H=[1 0 0 0;0 1 0 0];  

% the state vector  

X=zeros(StateParamNum,1);  

X=[D(1,1);D(2,1);0.001;0.001];  

% the control vector  

U=0*randn(2,1);  

% the observation vector  

Z=zeros(ObsevParamNum,1);  

% the covariance of thestate  

P=eye(StateParamNum,StateParamNum);  

P(1,1)=10;  

P(2,2)=10;  

P(3,3)=10;  

P(4,4)=10;  

% the covariance of thestate noise  

q=eye(StateParamNum,StateParamNum);  

q(1,1)=0.1;  

q(2,2)=0.1;  

q(3,3)=0.01;  

q(4,4)=0.01;  

% the covariance of theobserve noise  

r=eye(ObsevParamNum,ObsevParamNum);  

r(1,1)=10;  

r(2,2)=10;  

% the optimal estimationof the the state  

Xf=zeros(StateParamNum,lens);  

% the optimal estimationof the the observation  

Zf=zeros(ObsevParamNum,lens);  

V=zeros(ObsevParamNum,lens);  

Pf=zeros(StateParamNum,lens);  

for i=1:lens  

    % theestimation of the state in time t  

    Xest=F*X+G*U;  

    % thecovariance of the estimated state  

    Pest=F*P*F'+q;  

    % theestimation of the observation in time t  

    Zest=H*Xest;  

    % thecovariance of the estimated observation  

    Sest=H*Pest*H'+r;  

    % theKalman Gain  

    K=Pest*H'*inv(Sest);  

    % thedifference between estimation and observation  

    v=D(:,i)-Zest;  

    % theoptimal estimation of the state in time t  

    X=Xest+K*v;  

    % thecovariance of the optimal state  

   P=(eye(StateParamNum,StateParamNum)-K*H)*Pest;  

    % theoptimal estimation of the observation in time t  

    Z=H*X;  

    Xf(:,i)=X;  

    Zf(:,i)=Z;  

    V(:,i)=v;  

    Pf(:,i)=diag(P);  

end  

figure(1)  

hold on  

colormax=lens+1;  

c=(1:lens-1)/colormax;  

c1=repmat(c,3,1);  

c2=[ones(1,lens-1);repmat(c,2,1)];  

for i=1:lens-1  

    plot(D(1,i:i+1),D(2,i:i+1),...  

        'LineWidth',3,'Color',c1(:,i)');  

    plot(Zf(1,i:i+1),Zf(2,i:i+1),'-+',...  

        'LineWidth',1,'Color',[1 0 0]);  

end  

hold off

a b

图4 a、原始数据和滤波数据；b、拟合误差

上面这段代码中去掉了控制部分，调整q和r可以改善滤波的效果。

在上面的推导过程中，对Kalman滤波有以下几点认识

1、模型是线性的，体现在公式（4）中；

2、模型是高斯的，体现在公式（4）、（5）中；

3、式（4）中的F和G矩阵没有更新；

4、状态变量是根据设计者的知识给出的。

综合以上几点，Kalman滤波是一个预设的跟踪器，物体的运动模型，运动之间的关系都是给定的。

我们可以默认这些预设都是正确的，直接来用。但是，人作为第一发现者，是如何从数据中抽象出这些状态，如何从状态到状态的转移求得运动模型，这些都没有解决。如果，这个问题没有解决，我们将无法进入下一个螺旋上升的阶段。说到这里，我不得不怀疑这样一点——我们获得的所谓的运动模型，所谓的状态变量的内容，是不是某些非人类的文明灌输给我们的。如果是这样的，那么我们不可能发现螺旋上升的途径；如果不是这样，这些是我们自己发现的，那么我们就有办法重新发现“发现知识”的过程，指导我们进入下一个螺旋。