贝叶斯滤波（五）卡尔曼滤波算法推导

最新推荐文章于 2024-09-06 18:30:32 发布

Luke-Wong

最新推荐文章于 2024-09-06 18:30:32 发布

阅读量3.3k

点赞数 4

分类专栏：贝叶斯滤波

本文链接：https://blog.csdn.net/erzhonghou0033/article/details/106785540

版权

贝叶斯滤波专栏收录该内容

4 篇文章 9 订阅

订阅专栏

一、回顾贝叶斯滤波

1. 贝叶斯滤波算法到卡尔曼滤波

根据贝叶斯滤波（三）贝叶斯滤波算法推导

我们把贝叶斯滤波推导的结论写出来：

$bel(x_{t})=\eta p(y_{t}|x_{t})\int_{-\infty }^{+\infty}p(x_{t}|u_{t},x_{t-1})bel(x_{t-1})dx_{t-1}$

其中

$\eta =\frac{1}{\int_{-\infty }^{+\infty}p(y_{t}|x_{t})\int_{-\infty }^{+\infty}p(x_{t}|x_{t-1})bel(x_{t-1})dx_{t-1}dx}$

我们把 $\overline{bel}(x_{t})$ 替代式中广义积分那一项，我们叫做 $x_{t}$ 的先验：

$\overline{bel}(x_{t})=\int_{-\infty }^{+\infty}p(x_{t}|u_{t},x_{t-1})bel(x_{t-1})dx_{t-1}$

则，

$bel(x_{t})=\eta p(y_{t}|x_{t})\overline{bel}(x_{t})$

求解 $bel(x_{t})$ 这个等式，其实也就是分两步求解：

第一步便是求解 $\overline{bel}(x_{t})$ ，通常把这一步称为预测步。

第二步就是求解 $\eta p(y_{t}|x_{t})\overline{bel}(x_{t})$ ，通常把这步称为更新步。

假设 $p(y_{t}|x_{t}),p(x_{t}|u_{t},x_{t-1})$ 都服从高斯分布的话，那么这就是人们常说的卡尔曼滤波。

推导这个卡尔曼滤波结果也是非常简单的了，根据贝叶斯滤波（四）两个高斯分布函数相乘、卷积推导：

预测步求解 $\overline{bel}(x_{t})$ 相当于求解高斯分布的卷积;

更新步求解 $\eta p(y_{t}|x_{t})\overline{bel}(x_{t})$ 就是求解两个高斯分布的乘积，而这个计算我们在上一节便已经推导过了。

通过这两步的计算很容易就可以推导出 $bel(x_{t})$

二、卡尔曼滤波推导

1. 线性高斯系统

卡尔曼滤波有三个假设：

(1)第一是状态转移概率带有随机高斯噪声的参数的线性函数，本章推导假定状态矩阵是一维的：

$x_{t}=a_{t}x_{t-1}+b_{t}u_{t}+ \varepsilon_{t}$

其噪声满足高斯分布

$\varepsilon_{t} \sim \mathcal N(0, {{\sigma}^{2}_{Q}}_{,t})$

那么由于 $x_{t}=a_{t}x_{t-1}+b_{t}u_{t}+ \varepsilon_{t}$ ，这个式子相当于一个高斯分布 $\varepsilon_{t}$ 加上 $a_{t}x_{t-1}+b_{t}u_{t}$ ，因此 $x_{t}$ 也满足高斯分布，所以

$p(x_{t}|u_{t},x_{t-1})\sim \mathcal N(a_{t}x_{t-1}+b_{t}u_{t}, {{\sigma}^{2}_{Q}}_{,t})$

(2)第二是观测概率的噪声也必须是带有随机高斯噪声的参数的线性函数的

$y_{t}=c_{t}x_{t}+\delta_{t}$

其噪声满足高斯分布

$\delta_{t} \sim \mathcal N(0, {{\sigma}^{2}_{R}}_{,t})$

那么 $p(y_{t}|x_{t})$ 满足以下的高斯分布：

$p(y_{t}|x_{t})\sim \mathcal N(c_{t}x_{t}, {{\sigma}^{2}_{R}}_{,t})$

(3)第三是初始状态的 $bel(x_{0})$ 必须满足高斯分布

$bel(x_{0})\sim \mathcal N(\mu_{0}, \sigma_{0}^{2})$

前两个假设意思就是说，观测是状态的线性函数，并且下一个状态是以前状态的线性函数，并且方程是符合高斯的，这样的话，高斯随机变量的任何线性变换都将导致另一个高斯随机变量。第三个假设是为了确保初值符合高斯分布，这样往后递推出来的才能都是符合高斯分布。

这三个假设确保 $bel(x_{t-1})$ 是符合高斯分布，所以我们在这里给定其均值方差为：

$bel(x_{t-1})\sim \mathcal N(\mu_{t-1}, \sigma_{t-1}^{2})$

2. 预测步

预测步的目标是要求解 $\overline{bel}(x_{t})$ ，

$\overline{bel}(x_{t})=\int_{-\infty }^{+\infty}p(x_{t}|\mu_{t},x_{t-1})bel(x_{t-1})dx_{t-1}$

而我们已知了，

$bel(x_{t-1})\sim \mathcal N(\mu_{t-1}, \sigma_{t-1}^{2})$

$p(x_{t}|u_{t},x_{t-1})\sim \mathcal N(a_{t}x_{t-1+b_{t}u_{t}}, {{\sigma}^{2}_{Q}}_{,t})$

根据贝叶斯滤波（四）两个高斯分布函数相乘、卷积推导，

我们可以得到：

$\overline{bel}(x_{t})\sim \mathcal N(\overline{\mu_{t}}, \overline{\sigma_{t}^{2}})$

其中，

$\overline{u_{t}}=a_{t}\mu_{t-1}+b_{t}u_{t}$

$\overline{\sigma_{t}^{2}}=a_{t}^2\sigma_{t-1}^2+\sigma_{Q,t}^2$

3. 更新步

已知

$bel(x_{t})=\eta p(y_{t}|x_{t})\overline{bel}(x_{t})$

$\eta =\frac{1}{\int_{-\infty }^{+\infty}p(y_{t}|x_{t})\int_{-\infty }^{+\infty}p(x_{t}|x_{t-1})bel(x_{t-1})dx_{t-1}dx}$

根据贝叶斯滤波（四）两个高斯分布函数相乘、卷积推导中的(二、两个高斯分布函数相乘（用于推导卡尔曼滤波）)，可知有以下结论：

$X \sim \mathcal N(\mu_{1}, {{\sigma}}^{2}_{1})$

$Y|x \sim \mathcal N(cx, {{\sigma}}^{2}_{2})$

$p_{g} = p(x)p(y|x)=\frac{S_{g}}{\sqrt{2 \pi \sigma_{g}^{2}}}exp^{-\frac{1}{2} (\frac{ (x - \mu{g})^{2} } {\sigma_{g}^{2}} )}$ ， $\mu_{g} = \frac{\mu_{1}\sigma^{2}_{2}+cy\sigma^{2}_{1}}{c^2\sigma^{2}_{1}+\sigma^{2}_{2}},\sigma_{g}^{2} = \frac{{\sigma^{2}_{1}\sigma^{2}_{2}}} {c^2\sigma^{2}_{1}+\sigma^{2}_{2}}$

$S_{g}=\frac{1}{\sqrt{2 \pi (\sigma^{2}_{1}+\sigma^{2}_{2})}}exp^{-\frac{1}{2} (\frac{(\mu_{1}c-y)^2}{c^2\sigma^{2}_{1}+\sigma^{2}_{2}})}$

又已知，

$\overline{bel}(x_{t})\sim \mathcal N(\overline{\mu_{t}}, \overline{\sigma_{t}^{2}})$

$p(y_{t}|x_{t})\sim \mathcal N(c_{t}x_{t}, {{\sigma}^{2}_{R}}_{,t})$

代入可得，

$bel(x_{t})=\eta p(y_{t}|x_{t})\overline{bel}(x_{t})$
$=\eta \frac{S_{t}}{\sqrt{2 \pi \sigma_{t}^{2}}}exp^{-\frac{1}{2} (\frac{ (x - \mu{t})^{2} } {\sigma_{t}^{2}} )}$ ， $\mu_{t} = \frac{\overline{\mu_{t}}{{\sigma}^{2}_{R}}_{,t}+c_{t}y_{t}\overline{\sigma_{t}^{2}}}{c_{t}^2\overline{\sigma^{2}_{t}}+{{\sigma}^{2}_{R}}_{,t}},\sigma_{t}^{2} = \frac{{\overline{\sigma^{2}_{t}}{{\sigma}^{2}_{R}}_{,t}}} {c^2\overline{\sigma^{2}_{t}}+{{\sigma}^{2}_{R}}_{,t}}$

从这一步开始，便是为了消除 $S_{t}$ 。代入 $\eta$ ，

$=\frac{\frac{S_{t}}{\sqrt{2 \pi \sigma_{t}^{2}}}exp^{-\frac{1}{2} (\frac{ (x - \mu{t})^{2} } {\sigma_{t}^{2}} )}}{\int_{-\infty}^{+\infty}\frac{S_{t}}{\sqrt{2 \pi \sigma_{t}^{2}}}exp^{-\frac{1}{2} (\frac{ (x - \mu{t})^{2} } {\sigma_{t}^{2}} )}dx}$

$=\frac{\frac{S_{t}}{\sqrt{2 \pi \sigma_{t}^{2}}}exp^{-\frac{1}{2} (\frac{ (x - \mu{t})^{2} } {\sigma_{t}^{2}} )}}{S_{t}\int_{-\infty}^{+\infty}\frac{1}{\sqrt{2 \pi \sigma_{t}^{2}}}exp^{-\frac{1}{2} (\frac{ (x - \mu{t})^{2} } {\sigma_{t}^{2}} )}dx}$

上下约掉一个 $S_{t}$ ，

$=\frac{\frac{1}{\sqrt{2 \pi \sigma_{t}^{2}}}exp^{-\frac{1}{2} (\frac{ (x - \mu{t})^{2} } {\sigma_{t}^{2}} )}}{\int_{-\infty}^{+\infty}\frac{1}{\sqrt{2 \pi \sigma_{t}^{2}}}exp^{-\frac{1}{2} (\frac{ (x - \mu{t})^{2} } {\sigma_{t}^{2}} )}dx}$

把分母的倒数进行简化为 $\eta_{t}$ ，此处经过归一化之后，便满足概率公理了：

$=\eta_{t} \frac{1}{\sqrt{2 \pi \sigma_{t}^{2}}}exp^{-\frac{1}{2} (\frac{ (x - \mu{t})^{2} } {\sigma_{t}^{2}} )}$

根据，

$\mu_{t} = \frac{\overline{\mu_{t}}{{\sigma}^{2}_{R}}_{,t}+c_{t}y_{t}\overline{\sigma^{2}_{t}}}{c_{t}^2\overline{\sigma^{2}_{t}}+{{\sigma}^{2}_{R}}_{,t}}$

$= \frac{\overline{\mu_{t}}{{\sigma}^{2}_{R}}_{,t}+c_{t}y_{t}\overline{\sigma^{2}_{t}}+\overline{\mu_{t}}c_{t}^2\overline{\sigma^{2}_{t}}-\overline{\mu_{t}}c_{t}^2\overline{\sigma^{2}_{t}}}{c_{t}^2\overline{\sigma^{2}_{t}}+{{\sigma}^{2}_{R}}_{,t}}$

$= \frac{ {{\sigma}^{2}_{R}}_{,t}+c_{t}^2\overline{\sigma^{2}_{t}}}{c_{t}^2\overline{\sigma^{2}_{t}}+{{\sigma}^{2}_{R}}_{,t}}\overline{\mu_{t}} + \frac{c_{t}\overline{\sigma^{2}_{t}}}{c_{t}^2\overline{\sigma^{2}_{t}}+{{\sigma}^{2}_{R}}_{,t}}(y_{t}-\overline{\mu_{t}}c_{t})$

$= \overline{\mu_{t}} + \frac{c_{t}\overline{\sigma^{2}_{t}}}{c_{t}^2\overline{\sigma^{2}_{t}}+{{\sigma}^{2}_{R}}_{,t}}(y_{t}-\overline{\mu_{t}}c_{t})$

$= \overline{\mu_{t}} +K_{t}(y_{t}-\overline{\mu_{t}}c_{t})$

$K_{t}=\frac{c_{t}\overline{\sigma^{2}_{t}}}{c_{t}^2\overline{\sigma^{2}_{t}}+{{\sigma}^{2}_{R}}_{,t}}$

$\sigma_{t}^{2} = \frac{{\overline{\sigma^{2}_{t}}{{\sigma}^{2}_{R}}_{,t}}} {c^2\overline{\sigma^{2}_{t}}+{{\sigma}^{2}_{R}}_{,t}}$

$= \frac{{\overline{\sigma^{2}_{t}}{{\sigma}^{2}_{R}}_{,t}}+\overline{\sigma^{4}_{t}}c_{t}^2-\overline{\sigma^{4}_{t}}c_{t}^2} {c^2\overline{\sigma^{2}_{t}}+{{\sigma}^{2}_{R}}_{,t}}$

$= \frac{ { ({{\sigma}^{2}_{R}}_{,t}} +\overline{\sigma^{2}_{t}}c_{t}^2)\overline{\sigma^{2}_{t}} -(\overline{\sigma^{2}_{t}}c_{t}^2)\overline{\sigma^{2}_{t}} } { c^2\overline{\sigma^{2}_{t}}+{{\sigma}^{2}_{R}}_{,t} }$

$=\overline{\sigma^{2}_{t}} - \frac{ \overline{\sigma^{2}_{t}}c_{t}^2 } { c^2\overline{\sigma^{2}_{t}}+{{\sigma}^{2}_{R}}_{,t} }\overline{\sigma^{2}_{t}}$

$=(1- \frac{ \overline{\sigma^{2}_{t}}c_{t}^2 } { c^2\overline{\sigma^{2}_{t}}+{{\sigma}^{2}_{R}}_{,t} })\overline{\sigma^{2}_{t}}$

$= (1-K_{t}c_{t})\overline{\sigma^{2}_{t}}$

4. 推导结果

至此，卡尔曼滤波推导完毕，我们把预测步与更新步总结到一起：

预测步：

$\overline{u_{t}}=a_{t}\mu_{t-1}+b_{t}u_{t}$

$\overline{\sigma_{t}^{2}}=a_{t}^2\sigma_{t-1}^2+\sigma_{Q,t}^2$

更新步：

$K_{t}=\frac{c_{t}\overline{\sigma^{2}_{t}}}{c_{t}^2\overline{\sigma^{2}_{t}}+{{\sigma}^{2}_{R}}_{,t}}$

$\mu_{t} = \overline{\mu_{t}} +K_{t}(y_{t}-\overline{\mu_{t}}c_{t})$

$\sigma_{t}^{2} = (1-K_{t}c_{t})\overline{\sigma^{2}_{t}}$

这五条公式便是经典的卡尔曼滤波公式，与贝叶斯滤波很大不同的地方在于，卡尔曼滤波不需要计算各种广义积分，有的只是简单的加减乘除算法，极大地降低了计算量。

三、卡尔曼滤波的matlab实现

写了一个简单的一维卡尔曼滤波的例子：

% x_: 先验均值
% sigma2_: 先验方差
% x: 后验均值
% sigma2: 后验方差
% a: 状态转移方程
% b: 控制输入
% c: 观测方程
% Q: 过程噪声方差
% R: 观测方差
%
% 预测步：
%         x_ = a*x + b;
%         sigma2_ = a^2*sigma2 + Q;
% 更新步：
%         k = c*sigma2_ / (c^2*sigma2_ + R);
%         x = x_ + k(y - x_*c);
%         siamg2 = (1 - k*c)siamg2_;
 
% 设置数据长度为N
N = 200;
% 生成时间轴
t = (1:N);
% 真实信号
real_signal = zeros(1,N);
% 观测数据
z = zeros(1,N);
% 存储卡尔曼估计的结果，缓存起来用于画图形
x_filter = zeros(1,N);
% 存储卡尔曼预测中的增益k，缓存起来用于画图形
K = zeros(1,N);
% 状态转移方程
a = 1;
% 控制输入
b = 0;
% c: 观测方程
c = 1;
 
% 设置初值 初始的均值和方差
x = 5;
sigma2 = 10;
 
% 设置生成的信号的噪声标准差
V = 50;
% 设置状态转移方差Q和观测方差R
Q = 1;
R = 40;
 
% 初始化真实数据和观测数据
for i=1:N
   %生成真实数据，为1-100线性增加的信号
   real_signal(i) = i*2;
   %生成观测，真实数据基础上叠加一个高斯噪声 normrnd(均值, 标准差)
   z(i) = real_signal(i) + normrnd(0, V);
end
 
% 卡尔曼滤波
for i=1:N
    % 预测步
    x_ = a*x + b;
    sigma2_ = a*a*sigma2+Q;
    
    % 更新步
    k = c*sigma2_/(c*c*sigma2_+R);
    x = x_ + k*(z(i) - x_*c);
    sigma2 = (1-k*c)*sigma2_;
    
    % 存储滤波结果
    x_filter(i) = x;
    K(i) = k;
end
 
% 画出卡尔曼增益k 可以看到系统很快会达到稳态，k会很快收敛成为一个常数，此时卡尔曼滤波也就相当于低通滤波了
plot(t, K)
legend('K');
 
% 画出波形， 真实值 - 观测值 - 卡尔曼估计值
figure(2)
plot(t, real_signal, 'r', t, z, 'g', t, x_filter, 'b')
legend('real','measure','filter');

以下为个人见解，可能不是很对，有错误的地方还请各位朋友指正：

卡尔曼滤波其实没有那么神奇，毕竟卡尔曼增益最终会收敛为一个常数，此时的卡尔曼滤波其实跟低通滤波没什么区别。

但是卡尔曼滤波相当于给出了一个找到这个最佳低通滤波参数的框架：根据我们观测的变量的波形，我们可能能够发现其噪声的方差，根据这个，我们可以比较有方向地进行调试、调整Q和R，从而找出最佳的卡尔曼增益K，达到滤波最佳效果。