【状态估计】粒子滤波器、Σ点滤波器和扩展/线性卡尔曼滤波器研究（Matlab代码实现）
💥💥💞💞欢迎来到本博客❤️❤️💥💥
🏆博主优势：🌞🌞🌞博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。
⛳️座右铭：行百里者，半于九十。
📋📋📋本文目录如下：🎁🎁🎁
目录
💥1 概述
- 粒子过滤器通常需要大量粒子，这可能需要大量的运行时间。即使是最简单的粒子过滤器也使用 1000 个粒子的情况并不少见，每次测量需要 1000 次模拟。随着状态维度的增长，必要数量的粒子变得巨大。（我们的问题会从1000个粒子中受益匪浅，但要理解具有如此大量粒子的图会更难。
- 将不确定性表示为一组粒子和权重（离散概率分布）意味着状态的最佳估计通常非常粗糙，因此粒子过滤器对于需要高精度的问题效果不佳。
- 当需要更好的性能时，粒子滤波器通常必须进行大量定制，以适应每个单独的状态估计问题，这可能需要很长时间，尤其是因为测试需要运行过滤器，而过滤器本身可能需要很长时间。出于同样的原因，很难找到有用的通用粒子过滤器，尽管自举过滤器可以解决简单的问题。
在Σ点滤波器（也称为无迹滤波器）中，我们不用一大堆散射粒子来表示不确定性，而是假设不确定性具有高斯（正态）分布，并且以当前最佳估计值为中心：
因此，我们可以用协方差矩阵来表示不确定性，就像我们为上面的粒子计算的那样。我们将协方差可视化为围绕状态估计的椭圆，其中椭圆绘制在 3σ 边界处（因此，真实状态大约 99.7% 的时间在这个椭圆内）。绘制 1000 个粒子只是为了进行比较。
当各种不确定度源（先前的不确定度、过程噪声和测量噪声）是单峰且不相关的时，Σ点滤波器是一个强大的选择。一些优点：
- 在他们的假设中，它们通常比粒子过滤器更准确，因为它们不依赖于随机粒子。
- 它们比粒子过滤器快得多。粒子过滤器可能需要 1000 个点，而 Σ-point 过滤器可能只需要 9 个左右。
- 他们的假设适用于许多不同的实际问题，并且设置Σ点滤波器只需要定义传播函数，测量函数，过程噪声协方差和测量噪声协方差，所有这些都是粒子滤波器所必需的。
- Σ点滤波器有标准形式，因此在书籍或期刊中找到良好的参考相对容易。
不过，我们可以列出一些缺点。
- 奇怪的问题可能导致Σ点滤波器“分崩离析”。例如，在我们的球问题中，如果时间步长更大，那么在一次或两次反弹中，sigma 点将变得非常“混乱”，并可能导致样本协方差矩阵毫无意义。可能很难避免这种情况，而粒子过滤器不会有这个问题。
- 虽然它们比粒子滤波器快得多，但它们也比扩展卡尔曼滤波器慢得多，我们稍后会谈到。
📚2 运行结果
2.1 扩展卡尔曼滤波
2.2 线性卡尔曼滤波
2.3 粒子滤波
2.4 Σ点滤波器
部分代码：
%% Create the true system and show the initial filter state.
% Set the random number generator seed so the results are the same every
% time we run the script. (Comment out this line to see different results
% every time.)
rng(1);
% Initial true state, measurement noise covariance, and measurement
x0 = [0; 3; 1; 0];
R = 0.5^2 * eye(2);
z0 = x0(1:2) + covdraw(R);
% Initial state estimate and covariance
xh0 = [z0; 1; 0];
P0 = blkdiag(R, 2^2 * eye(2));
% Calculate the whole true trajectory.
[~, x, t] = propagate_ball(0, 10, x0);
% Prepare the figure.
set(clf(figure(1)), 'Color', [1 1 1]);
axis equal;
axis([-1 11 0 5]);
xlabel('x [m]');
ylabel('y [m]');
hold on;
% Draw the 3-sigma boundary for the uncertainty.
ell = ellipse(P0, xh0);
hP = plot(ell(1,:), ell(2,:), 'Color', 0.75 * [1 1 1]);
% Add particles for comparison only.
X = bsxfun(@plus, covdraw(P0, 1000), xh0);
hX = plot(X(1,:), X(2,:), '.', 'Color', 0.75 * [1 1 1]);
🎉3 参考文献
部分理论来源于网络，如有侵权请联系删除。
[1]柏庆文. 基于无味卡尔曼滤波的电动汽车动力电池SOC估计[D].吉林大学,2013.
[2]常国宾,许江宁,李安,常路宾.迭代无味卡尔曼滤波的目标跟踪算法[J].西安交通大学学报,2011,45(12):70-74.