案例导入：具有动态变化的连续变量的例子

物体轨迹跟踪（雷达、声纳、图像、声学）
导航
控制应用
生物医学应用
股票预测
金融产品的风险评估

状态空间（state space）

状态矢量（state vector）

状态被定义为预测该系统未来状态所必需的变量的集合，状态矢量 $\vec{s}(t)$ 依赖于时间 $t$ 。
系统的(状态)变化往往受到微分方程的支配 $\frac{d\vec{s}(t)}{dt}=\varphi(\vec{s}(t))$ 在线性微分方程(linear differential euqation)的情况下， $\varphi(\vec{s}(t))=\Phi\vec{s}(t)$ ，离散时间解的结果是一个线性确定性差分方程(linear deterministic difference equation)
$\vec{s}_{k+1}=F\vec{s}_{k}$
其中， $\vec{F}=\text{mexp} {-\vec{\Phi}(t_{k+1}-t_{k})}$
对于非线性离散时间解，我们可以得到一个非线性差分方程 $\vec{s}_{k+1} = f(\vec{s}_k)$

一般模型

为了模拟系统中的不确定性，我们引入了过程噪声（Process noise）这一概念。

演化方程（Evolution equation）

一般非线性离散时间随机动态系统（general nonlinear discrete-time stochastic dynamic system） $\vec{s}_{k+1} = f_k(\vec{s}_k, \vec{w}_k)$ 线性离散时间随机动态系统 $\vec{s}_{k+1} = \vec{F} \vec{s}_{k} + \vec{w}_{k}$

观测方程（Observation equation）

非线性测量模型 $\vec{z}_k = h_k(\vec{s}_k, \vec{n}_k)$ 线性测量模型 $\vec{z}_k=\vec{H}\vec{s}_k+\vec{n}_k$

估计（Estimation）

估计包含了滤波（Filtering）、预测（Prediction）和平滑（Smoothing）三个过程。我们在本文中关注的估计问题是滤波问题，其目标在于（递归）计算后验概率密度(Posterior probability density)。

滤波

根据目前的测量结果估计当前状态
$p(\vec{s}_k|\vec{Z}_k)$ 滤波包括(贝叶斯)递归方法，这里我们用一般状态空间公式解释贝叶斯递归这一问题。
针对一般（非线性离散时间）随机动态系统，演化方程和观测方程如下所示。
$\vec{s}_{k+1} = f_k(\vec{s}_k, \vec{w}_k)$ $\vec{z}_k = h_k(\vec{s}_k, \vec{n}_k)$ 这一问题可以通过两步法来解决：预测（prediction）和更新（update）。

预测

假设 $p(\vec{s}_k|\vec{Z}_k)$ 已知，那么预测的概率密度为
$p(\vec{s}_{k+1}|\vec{Z}_k)=\int_sp(\vec{s}_{k+1},\vec{s}_k|\vec{Z}_k)d\vec{s}_k=\int_sp(\vec{s}_{k+1}|\vec{s}_k)p(\vec{s}_k|\vec{Z}_k)d\vec{s}_k$

更新/改正

$p(\vec{s}_k|\vec{Z}_k)=\frac{p(\vec{z}_k|\vec{s}_k)p(\vec{s}_k|\vec{Z}_{k-1})}{p(\vec{z}_k|\vec{Z}_{k-1})}$

但是连续状态空间模型的贝叶斯递归一般没有闭式解（closed-form solution）!只有一种例外：线性高斯系统（Linear Gaussian System）。

因此我们考虑一般线性模型（General Linear model），这就是后文提到的卡尔曼滤波器。

预测

根据目前的测量结果估计未来的状态 $p(\vec{s}_{k+1}|\vec{Z}_k)$

平滑

根据目前的测量结果估计以前的状态 $p(\vec{s}_{l}|\vec{Z}_k), l<k$

卡尔曼滤波器（Kalman Filter）

在上文的滤波环节，我们介绍了连续状态空间模型的贝叶斯递归问题，但是很可惜，该问题一般没有闭式解，因此我们考虑其例外：线性高斯系统，演化方程和观测方程简化为如下所示。
$\vec{s}_{k+1} = \vec{F} \vec{s}_{k} + \vec{G}\vec{w}_{k}$ $\vec{z}_k=\vec{H}\vec{s}_k+\vec{n}_k$

回顾上面的贝叶斯递归，就会发现，如果初始密度是高斯的，那么所有的预测密度和滤波密度也是高斯的。

卡尔曼预测

我们假设后验概率密度是 $p(\vec{s}_k|\vec{Z}_k)$ , 进行 $\vec{y}=\vec{F}\vec{s}_k$ 线性变换，可以得到 $p(\vec{y}|\vec{Z}_k) = \mathcal{N}(\vec{y}, \vec{F}\vec{s}_{k|k},\vec{F} \vec{Q}_{k|k} \vec{F}^T)$
接着，在 $\vec{y}$ 上加高斯分布变量 $\vec{G}\vec{w}_k$ ，且 $p(\vec{w}_k)=\mathcal{N}(\vec{w}_k,\vec{0},\vec{W})$
这样我们可以得到一个高斯分布的变量

因此，卡尔曼滤波器中的预测步骤等于

\vec{s}_{k+1|k}=\vec{F}\vec{s}_{k|k} \tag{1}

\vec{Q}_{k+1|k}=\vec{F}\vec{Q}_{k|k}\vec{F}^T + \vec{G}\vec{W}\vec{G}^T \tag{2}

卡尔曼更新

此处， $p(\vec{s}_k|\vec{Z}_{k-1})$ 作为先验概率 (Prior probability)，让我们回顾一下贝叶斯更新公式

我们可以重新使用这些推导过的方程，替换为

\vec{s}_{k|k}=\vec{s}_{k|k-1}+\vec{K}_k(\vec{z}_k-\vec{H}\vec{s}_{k|k-1}) \tag{3}

\vec{Q}_{k|k}=(\vec{I}-\vec{K}_k\vec{H})\vec{Q}_{k|k-1} \tag{4}

\vec{K}_k = \vec{Q}_{k|k-1}\vec{H}^T(\vec{H}\vec{Q}_{k|k-1}\vec{H}^T+\vec{N}_k)^{-1} \tag{5}

综上，卡尔曼方程共五条，分别是卡尔曼预测和卡尔曼更新方程。

仿真代码（Simulation）

'''

% This file simulates a Kalman Filter on a 1D constant velocity linear Gaussian estimation problem. 

% Initial position and speed of an object
x = 0;
v = 50;

% Measurement Interval
T = 2;

% Observation and process noise variance
varn = 100^2; % Observation noise variance
varw = 1^2; % Process noise variance 
K = 100
t = 0:T:K*T;

% Generate a set of position measurements
x_true = x+t*v;
xm = x_true+sqrt(varn)*randn(size(t)); % measurements z_k

% Initial state of the Kalman filter     
sp = [100; 0]; % s_{k|k-1}
Qp = diag([1E6 1E4]); %Q_{k|k-1}

% Definition of system matrices: 
% H = Observation matrix, 
% F = System Dynamics Matrix
% W = part of Process Noise Covariance matrix

H = [1 0];

F = [1 T; 0 1];
%C = [T^2/2; T];
W = [T^3/3 T^2/2; T^2/2 T];

for k=1:K

    k
    % KF update

    G = Qp*H'*inv(H*Qp*H' + varn); % Kalman gain K_k  
    sf = sp + G*(xm(k)-H*sp); % s_{k|k}
    Qf = (eye(2)-G*H)*Qp; % Q_{k|k}
    mean_KF(:,k) = sf;
    sigmas_KF(:,k) = sqrt(diag(Qf));'''

动态状态估计 - 卡尔曼滤波 (Kalman Filter)

动态状态估计 - 卡尔曼滤波 (Kalman Filter)

案例导入：具有动态变化的连续变量的例子

状态空间（state space）

状态矢量（state vector）