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

待快雪时晴
460 阅读4分钟

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

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

状态空间（state space）

状态矢量（state vector）

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

一般模型

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

演化方程（Evolution equation）

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

观测方程（Observation equation）

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

估计（Estimation）

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

滤波

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

预测

假设p(s⃗k∣Z⃗k)p(\vec{s}_k|\vec{Z}_k)已知，那么预测的概率密度为 p(s⃗k+1∣Z⃗k)=∫sp(s⃗k+1,s⃗k∣Z⃗k)ds⃗k=∫sp(s⃗k+1∣s⃗k)p(s⃗k∣Z⃗k)ds⃗kp(\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(s⃗k∣Z⃗k)=p(z⃗k∣s⃗k)p(s⃗k∣Z⃗k−1)p(z⃗k∣Z⃗k−1)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(s⃗k+1∣Z⃗k)p(\vec{s}_{k+1}|\vec{Z}_k)

平滑

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

卡尔曼滤波器（Kalman Filter）

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

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

卡尔曼预测

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

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

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

Q⃗k+1∣k=F⃗Q⃗k∣kF⃗T+G⃗W⃗G⃗T\vec{Q}_{k+1|k}=\vec{F}\vec{Q}_{k|k}\vec{F}^T + \vec{G}\vec{W}\vec{G}^T

卡尔曼更新

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

μ=μx+K(z−Hμx)\mu = \mu_x + K(z-H\mu_x)

Σ=(I−KH)Σx\Sigma = (I - KH)\Sigma_x

K=ΣxHT(HΣxHT+Σn)−1K = \Sigma_x H^T (H\Sigma_xH^T + \Sigma_n)^{-1}

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

s⃗k∣k=s⃗k∣k−1+K⃗k(z⃗k−H⃗s⃗k∣k−1)\vec{s}_{k|k}=\vec{s}_{k|k-1}+\vec{K}_k(\vec{z}_k-\vec{H}\vec{s}_{k|k-1})

Q⃗k∣k=(I⃗−K⃗kH⃗)Q⃗k∣k−1\vec{Q}_{k|k}=(\vec{I}-\vec{K}_k\vec{H})\vec{Q}_{k|k-1}

K⃗k=Q⃗k∣k−1H⃗T(H⃗Q⃗k∣k−1H⃗T+N⃗k)−1\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}

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

仿真代码（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];
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));
    
    %% Plot 
    figure(1)
    plot ((1:k)*T, mean_KF(1,1:k)-x_true(1:k), 'r+')
    hold 
    plot ((1:k)*T, xm(1:k)-x_true(1:k), 'b+')
    plot ((1:k)*T, 3*sigmas_KF(1,1:k), 'r-.')
    plot ((1:k)*T, -3*sigmas_KF(1,1:k), 'r-.')
    hold
    title ('Position estimation error of KF')
    legend ('KF-error','measurements','3-sigma CR-bound')
    axis ([0 K*T -3*sigmas_KF(1,1) 3*sigmas_KF(1,1)])
   
    figure(2)
    plot ((1:k)*T, mean_KF(2,1:k)-v, 'r+')
    hold 
    plot ((1:k)*T, 3*sigmas_KF(2,1:k), 'r-.')
    plot ((1:k)*T, -3*sigmas_KF(2,1:k), 'r-.')
    hold
    title ('Velocity estimation error of KF')
    legend ('KF-error', '3-sigma CR-bound')
    axis ([0 K*T -3*sigmas_KF(2,1) 3*sigmas_KF(2,1)])
   
    %%  KF prediction 
    sp = F*sf; %s_{k+1|k}
    Qp = F*Qf*F' + W*varw; %Q_{k+1|k}
    
    pause
end

figure(10)

plot (x_true)
hold
plot (xm, 'x')
plot (mean_KF(1,:), 'o')

hold
avatar
文章
阅读
粉丝