其他算法-卡尔曼滤波器

最新推荐文章于 2022-08-17 12:16:53 发布

tzc_fly

最新推荐文章于 2022-08-17 12:16:53 发布

阅读量417

点赞数 5

分类专栏：白景屹-算法栈文章标签：算法

本文链接：https://blog.csdn.net/qq_40943760/article/details/111357547

版权

白景屹-算法栈专栏收录该内容

29 篇文章 4 订阅

订阅专栏

卡尔曼滤波器以Rudolf Kalman命名，是一种优化估算算法，用于存在测量误差情况下预测参数，广泛用于控制科学，计算机视觉，信号处理等领域。
注意：在了解卡尔曼滤波器之前需要先知道状态观测器

状态观测器

航天器从地球前往火星，需要时刻监测反应室的温度，如果温度过高不采取措施会导致其他设备出现故障，然而不能直接把温度传感器放置在反应室内，必须放在反应室外部的低温表面测量温度；
fig1
根据热传导的理论，可以联想到以下数学模型，输入量是燃油流量 $W_{fuel}$ ，中间状态是反应室温度 $T_{in}$ ，可观测的量是反应室外部的温度 $T_{ext}$ ：
fig2
记变量 $\widehat{x}$ 表示对变量 $x$ 的估计，则对于以上的实际系统，输入的量相同，当实际测量的输出量与热传导数学模型计算的输出很接近时，可以认为这个无法测量的 $T_{in}$ 与模型计算得到 $\widehat{T}_{in}$ 也是相接近的：

fig3
但现在的问题是如何让这个模型计算的输出量 $\widehat{T}_{ext}$ 越来越收敛于实际测量的 $T_{ext}$ ，这时就需要用一个闭环的反馈来实现，这就构成了状态观测器（蓝色部分）：
fig4
现在面临的问题就变成确定控制器K的参数，使 $\widehat{T}_{ext}$ 快速收敛于 $T_{ext}$ ；

值得一提，现在以现代控制理论的方式理解这个系统：
fig5
状态求差并化简后有：
$\dot{e}_{obs}=(A-KC)e_{obs}$
反拉普拉斯变换可以得到状态方程的解：
$e_{obs}(t)=e^{(A-KC)t}e_{obs}(0)$
可以看出，如果 $(A - K C) < 0$ ，则误差最终将收敛至0：
fig6

卡尔曼滤波算法的原理

为了简单分析，重新考虑一个在一维上的驾驶系统，输入为当前时刻 $k$ 的速度 $u_{k}$ ，输出为所在位置 $y_{k}$ ，并令输出等于当前的状态即 $y_{k}=Cx_{k},C=1$ ；
如果考虑坏境因素，测量位置时加入测量误差 $v_{k}$ ；驾驶中，状态会受到风速于路况影响即加入过程误差 $w_{k}$ ，将状态空间离散化表示为：
fig7

根据统计，测量误差与过程误差一般都服从正态分布；
假设本问题中：
$w\sim N(0,Q),v\sim N(0,R)$

现在单纯想构造状态观测器，观测到状态(驾驶系统所在位置)：
fig8
可以看出，在现实的系统中，存在着不确定性的误差，而驾驶模型本身不会考虑误差，这就导致估计状态 $\widehat{x}_{k}$ 由于误差的积累而难以收敛到测量的状态 $x_{k}$ ，此时就需要卡尔曼滤波算法，融合两者信息得到一个新的最优估计；
实际上卡尔曼滤波器本身就是一个状态观测器，只不过它是专为随机系统设计的。常规的状态观测器用于确定性的系统：
fig9
对于使用卡尔曼滤波器得到的估计状态：
fig10
第一部分：
$A\widehat{x}_{k-1}+Bu_{k}$
被称为先验估计，记为 $\widehat{x}_{k}^{-}$ ，其中 $\widehat{x}_{k-1}$ 来自数学模型的估计；
第二部分：
$K_{k}(y_{k}-C\widehat{x}_{k}^{-})$
其中 $y_{k}$ 来自于测量的输出量，第一部分加第二部分计算后的结果 $\widehat{x}_{k}$ 被称为后验估计；

卡尔曼滤波算法过程

上面式子中，还有 $K_{k}$ 要计算，算法过程分两步：
首先，数学模型用于计算状态的先验估计：
$\widehat{x}_{k}^{-}=A\widehat{x}_{k-1}+Bu_{k}$
$P_{k}^{-}=AP_{k-1}A^{T}+Q$
下一步，更新：
$K_{k}=\frac{P_{k}^{-}C^{T}}{CP_{k}^{-}C^{T}+R}$
$\widehat{x}_{k}=\widehat{x}_{k}^{-}+K_{k}(y_{k}-C\widehat{x}_{k}^{-})$
$P_{k}=(I-K_{k}C)P_{k}^{-}$

注意， $P, Q, R$ 分别为状态协方差矩阵，过程误差协方差矩阵，测量噪声协方差矩阵；

方差，协方差，协方差矩阵

对于随机变量 $X$ ，采样到 $n$ 个样本， $\overline{X}$ 为样本的均值(期望是对于概率而言的)，其方差为：
$var(X)=\frac{\sum_{i=1}^{n}(X_{i}-\overline{X})(X_{i}-\overline{X})}{n-1}$
而对于两个不同的随机变量 $X$ 和 $Y$ ，则协方差为：
$Cov(X,Y)=\frac{\sum_{i=1}^{n}(X_{i}-\overline{X})(Y_{i}-\overline{Y})}{n-1}$
一般地，对于随机变量组成的向量 $X=\left \{ X_{1},X_{2},...,X_{N} \right \}$ ，称矩阵 $C$ 为协方差矩阵：
fig11
其中， $c_{i,j}=Cov(X_{i},X_{j})$ ；
注意：

均值反映了样本分布的平均；
方差描述样本分布的离散程度，也就是该变量对于其期望的距离；
协方差表示的是两个变量的总体的误差，这与只表示一个变量误差的方差不同。如果两个变量的变化趋势一致，也就是说如果其中一个大于自身的期望值，另外一个也大于自身的期望值，那么两个变量之间的协方差就是正值。如果两个变量的变化趋势相反，即其中一个大于自身的期望值，另外一个却小于自身的期望值，那么两个变量之间的协方差就是负值；
协方差矩阵的每个元素是各个向量元素之间的协方差，是从标量随机变量到高维度随机向量的自然推广；

现在可以知道卡尔曼滤波算法中的 $P, Q, R$ 如何计算：
1.对于状态协方差矩阵 $P$ ：状态协方差矩阵 $P$ 就是各个状态之间的协方差组成的矩阵；
2.对于过程误差协方差矩阵 $Q$ ：该矩阵的每一个元素分别是各个状态的过程误差之间的协方差，各个状态的误差不容易确定，对于具体问题有具体的定义；比如对于车辆行驶，假设状态方程中有位置与速度两个状态，状态的误差可能需要考虑风力与地面摩擦力，力影响着加速度，所以要将误差从加速度推导到速度再推导到位移，最后根据两状态间的过程误差，计算协方差矩阵；
3.对于测量噪声协方差矩阵 $R$ ：来源是传感器对于可测量变量的误差，在使用时一般传感器都会给出精度指标，该指标就可以直接转化到矩阵 $R$ 中；

基于Matlab的卡尔曼滤波器实例

假设现在要测量一个静止区域的温度，数学模型也很简单：房间当前温度真实值为24度，认为下一时刻与当前时刻温度相同；
没有输入量即 $u = 0$ ，令输出量温度即为状态，状态只有一个，由此可建立模型：
$x_{k}=x_{k-1}$
$y_{k}=x_{k}$
在实际系统中，存在由于外界环境导致的过程误差 $Q$ 和温度计的测量误差 $R$ ，两个误差均服从正态分布，现在要用卡尔曼滤波器对温度计的测量值进行优化估计，模拟如下：

clear all;
% 房间当前温度真实值为24度，认为下一时刻与当前时刻温度相同，误差为0.02度，即认为连续的两个时刻最多变化0.02度。
% 温度计的测量误差为0.5度。
% 开始时，房间温度的估计为23.5度，误差为1度。
close all;

% intial parameters
% 计算连续n_iter个时刻
n_iter = 100;
% size of array. n_iter行，1列
sz = [n_iter, 1];
% 温度的真实值
x = 24;
% 过程方差，反应连续两个时刻温度方差
Q = 0.01;
% 测量方差，反应温度计的测量精度
R = 0.25;
% z是温度计的测量结果，在真实值的基础上加上了方差为0.25的高斯噪声。
z = x + sqrt(R)*randn(sz);
% 对数组进行初始化
% 对温度的后验估计。即在k时刻，结合温度计当前测量值与k-1时刻先验估计，得到的最终估计值
xhat = zeros(sz); 
% 后验估计的方差
P = zeros(sz); 
% 温度的先验估计。即在k-1时刻，对k时刻温度做出的估计
xhatminus = zeros(sz);
% 先验估计的方差
Pminus = zeros(sz);
% 卡尔曼增益，反应了温度计测量结果与过程模型（即当前时刻与下一时刻温度相同这一模型）的可信程度
K = zeros(sz); 
% intial guesses
xhat(1) = 23.5; %温度初始估计值为23.5度
P(1) = 1; % 误差为1

for k = 2:n_iter
    % 时间更新（预测）
    % 用上一时刻的最优估计值来作为对当前时刻的温度的预测
    xhatminus(k) = xhat(k-1);
    % 预测的方差为上一时刻温度最优估计值的方差与过程方差之和
    Pminus(k) = P(k-1)+Q;
    % 测量更新（校正）
    % 计算卡尔曼增益
    K(k) = Pminus(k)/( Pminus(k)+R );
    % 结合当前时刻温度计的测量值，对上一时刻的预测进行校正，得到校正后的最优估计。该估计具有最小均方差
    xhat(k) = xhatminus(k)+K(k)*(z(k)-xhatminus(k));
    % 计算最终估计值的方差
    P(k) = (1-K(k))*Pminus(k);
end

FontSize = 14;
LineWidth = 1;
figure();
plot(z,'ro-','LineWidth',LineWidth); %画出温度计的测量值
hold on;
plot(xhat,'bo-','LineWidth',LineWidth) %画出最优估计值
hold on;
plot(x*ones(sz),'g-','LineWidth',LineWidth); %画出真实值
legend('温度计的测量结果', '后验估计', '真实值');
xl = xlabel('时间(分钟)');
yl = ylabel('温度');