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

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

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

状态空间（state space）

状态矢量（state vector）

状态被定义为预测该系统未来状态所必需的变量的集合，状态矢量依赖于时间。
系统的(状态)变化往往受到微分方程的支配 在线性微分方程(linear differential euqation)的情况下，，离散时间解的结果是一个线性确定性差分方程(linear deterministic difference equation)

其中，
对于非线性离散时间解，我们可以得到一个非线性差分方程

一般模型

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

演化方程（Evolution equation）

一般非线性离散时间随机动态系统（general nonlinear discrete-time stochastic dynamic system） 线性离散时间随机动态系统

观测方程（Observation equation）

非线性测量模型 线性测量模型

估计（Estimation）

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

滤波

根据目前的测量结果估计当前状态
滤波包括(贝叶斯)递归方法，这里我们用一般状态空间公式解释贝叶斯递归这一问题。
针对一般（非线性离散时间）随机动态系统，演化方程和观测方程如下所示。
这一问题可以通过两步法来解决：预测（prediction）和更新（update）。

预测

假设已知，那么预测的概率密度为

更新/改正

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

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

预测

根据目前的测量结果估计未来的状态

平滑

根据目前的测量结果估计以前的状态

卡尔曼滤波器（Kalman Filter）

在上文的滤波环节，我们介绍了连续状态空间模型的贝叶斯递归问题，但是很可惜，该问题一般没有闭式解，因此我们考虑其例外：线性高斯系统，演化方程和观测方程简化为如下所示。

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

卡尔曼预测

我们假设后验概率密度是, 进行线性变换，可以得到
接着，在上加高斯分布变量，且
这样我们可以得到一个高斯分布的变量

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

卡尔曼更新

此处， 作为先验概率 (Prior probability)，让我们回顾一下贝叶斯更新公式

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

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

仿真代码（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));'''