前记
这是我个人在学习卡尔曼滤波过程中，对其的一点浅薄理解，一点想法。希望能为后人，作参考一二。
平均值
世界是嘈杂的。Kalman and Bayesian Fliter 这本书中如此说道。诚然，我们无法获得一个精确的数，世界总在和我们开名为误差的玩笑。只是误差，就好像命运石之门中时间线的收束，是有一个误差范围的。
根据大数定理，按照正态分布在正确值两侧的误差，在全部互加后，应当等于0.
（想象一下一堆石头，偏冷或者偏热，但是全部混一起就温度刚刚好）
因此我们对获取的数据作平均数，也就是(x1+x2+x3+….xn)/n，理论上就可以得到完美的数据。
你这么做了吗？那恭喜你，你获得了一个均值滤波器。当然，因为我们实际传感器是离散的，并且不可能等待收集足够多数据再取平均数。我们需要一边收集数据，一边进行平均，这样就需要一个新的公式
mean_new=(S_old+x_new)/n+1,也可以写成：
mean_new=(mean_oldn+x_new)/(n+1),然后
mean_new=(mean_oldn+mean_old-mean_old+x_new)/(n+1)
=(mean_old*(n+1)-mean_old+x_new)/(n+1)
即mean_new=mean_old+(x_new-mean_old)/(n+1)
这样我们就得到了一个比较简洁的公式，只需要储存平均值和数字个数，再加一个新数据就足够了
与此同时，数据会不断更新，我们要不断丢弃旧的数据，这会导致我们要加非常多次的数据
(时间复杂度O(n)，也就是指数级增长的计算量），因此我们需要让每一个新进来的家伙自己找好定位，而不是让整体去适应它。
那么我们便做点数学手段：
假设我们设置一个“窗口”，只对某个有n个数的范围内的数作平均，那么便有
S_new=S-x_old +x_new
mean_new=S_new/n
这个等价于：（S-x_old）/n+ x_new/n,也就是mean_new=mean_old+(x_new-x_old)/n
那么这便就是简单的滑动均值滤波器。
G-H 滤波器
我们知道，传感器是不准确的。它的读数会在一个范围内波动。但是，经验告诉我们，这个误差是在一个范围内的。假设我们一天后再称一次体重，体重秤的误差在10kg左右，获得的读数是50kg，那么我们就可以确定我们的体重至少是在40-60kg之间。假设我们再称一次体重，我们获得了51kg的读数，那么我们又可以获得一个41-61kg的范围。那么，我们的体重是增加了，还是减少，亦或者压根没变？ 无从得知。我们只能判定，我们最多上升了21kg，或者下降了19kg。下面的图很好的展示了，我们在一段连续的测量中能获得的变化趋势。
显然我们不会在一天之内变成月半（良子（恶俗））。因此，有时候一些夸张的数据是不可靠的。比如原本是50kg，因为误差漂到了60kg，这是不现实的。这时候我们根据自己的经验，可以对趋势做个估计，比如假设一天增重1kg(只是个随手举的例子)，那么30天后我们将增重30kg。这是真实可信的吗？我们不知道，但是我们“预测”如此。实际情况下，我们甚至可能体重减轻了。因此我们要使用测量取得的值作为参考。 然后我们就可以假设一个初始值x_0,然后对它以设定好的斜率预测一个步长，得到x_1，比如50（初始值）+1（步长）*1（斜率）=51（新预测值）。显然这个值不会和测量值一模一样，那么我们便将测量值和预测值做个差，得到Erro_x。（文献里一般把z和y当测量值，但是我这是菜狗小笔记）我们有多信任测量值呢？如果我们十分信任的话，我们就应该将这个差整个加到初始值中。如果我们完全不信任他，就应该直接使用预测值。如果我们半信不信呢?比如设置信任度为0.2，那么就将这个差乘以0.2，然后加到预测值上，我们就得到了“估计值”。接下来，我们再对这个估计值进行预测，然后再乘以信任度，得到新的估计值。 到这一步，我们已经初步完成了简单的G-H滤波器的框架。下面我将先给出一个相对完整的MATLAB示例代码：
dt = 1;
x = 0:dt:100;
h = 0.02;
g = 0.2;
% 生成一个线性增长的真实信号（初始值5，速度2）
mean_val = 5 + 2*x;
mean_add = 5 + (2+x).*x;
% 在真实信号上添加高斯白噪声，得到测量值
noise = mean_val + 100*randn(size(x));
% --- 3. 定义 G-H 滤波器函数 ---
% 这个函数逻辑现在是正确的 alpha-beta 形式
function gh_filt = gh_filter(g, h, x0, dt, dx, data)
th_x = x0; % 状态（位置）的初始估计
% 预分配内存以提高效率
gh_filt = zeros(size(data));
count = 0;
for z = data
% 预测步骤：根据上一刻的估计，预测当前位置
pre_x = th_x + dt * dx;
% 更新步骤
erro_x = z - pre_x; % 残差 = 测量值 - 预测值
% 更新导数（速度）
dx = dx + h * erro_x / dt;
% 更新状态（位置）- 使用标准的g-h滤波器更新公式
th_x = pre_x + g * erro_x;
count = count + 1;
gh_filt(count) = th_x; % 保存当前估计值
end
end
% --- 4. 调用滤波器并绘图 ---
% 初始估计：位置为5，速度为2
result = gh_filter(g, h, 5, dt, 2, noise);
% 绘图
figure; % 创建一个新窗口
plot(x, noise, 'o', 'MarkerSize', 3, 'DisplayName', '原始测量数据');
hold on;
plot(x, result, 'r-', 'LineWidth', 2, 'DisplayName', 'G-H 滤波结果');
plot(x, mean_val, 'k--', 'LineWidth', 1.5, 'DisplayName', '真实值（无噪声）');
hold off;
grid on;
title('G-H 滤波器效果（数据修正后）');
xlabel('时间 (t)');
ylabel('值');
legend('show'); % 显示图例
g参数就是对预测的信任程度，g大则更相信预测，否则更相信读取的数据。
h参数就是对变化的敏感程度。h大更快收敛，也更快震荡。
而我们的卡尔曼滤波器，包括许多其他的滤波器，都在数学上等价于G-H滤波器，不过它们用特别的办法，自动管理G,H参数，以解决G-H滤波器的一些问题
贝叶斯公式
贝叶斯公式，大伙都很熟悉，是关于先验概率和后验概率的。这里先引入几个专有名词：
似然，先验概率，后验概率。
似然，有专门的英文名词likelihood，直译过来的话是“可能性”的意思。实际意思也差不多，只不过不能和“概率”（possibility）画等号。它描述的是证据对假设的支持程度，是可以合起来不等于1的。比如说，你的证据可以证明某基因得A病率为50百分之，得B病率为60百分之，那么你有50百分之的似然得A病，60百分之的似然得B病，这个百分号只是表示证据对这个判断的可信度。
先验概率(Prior Probability)，在数学上有特别的含义，我这里先跳过，后面再仔细讨论。（本人数学很差劲喵，有错记得指出来）在这里，他表示的是对事件发生的预先估计，比如10个位置，在啥也不知道的情况下默认每个位置概率相等，为0.1。
后验概率(Posterior Probability)，同样先讨论代码意义。这里，他表示的是根据后来的验证，对先验概率进行修正后的概率。比如我确认在位置7的似然为0.8，在位置8/6的似然为0.1，那么将这个乘到原来的概率分布图上，我们就得到了[0.1,0.1,0.1,0.1,0.1,0.01,0.08,0.01,0.1,0.1]。看起来会比较匪夷所思，不是吗？
注：
有没有发现什么问题？我们得到了后验概率，但是对应位置的概率反而变小了。
这是因为似然没有覆盖所有地方。似然的形式根据测量的不同将非常不一样。如果我们将其他位置的似然都当做0.01,那么就可以得到归一化后的:
位置7的最终概率: 0.080 / 0.106 ≈ 75.5%
位置6和8的最终概率: 0.010 / 0.106 ≈ 9.4%
其他位置的最终概率: 0.001 / 0.106 ≈ 0.9%
但是，也请注意，不要混淆了这个[0.1,0.8,0.1]和下面代码中的卷积核，我们在示例中并不是这样使用这个似然的，我们将[0.1,0.8,0.1]作为卷积核来考虑。卷积的意思就是，用一个函数去修改另外一个函数。
卷积的python代码示例如下：
def predict_move_convolution(pdf, offset, kernel):
N = len(pdf)
kN = len(kernel)
width = int((kN - 1) / 2)
prior = np.zeros(N)
for i in range(N):
for k in range (kN):
index = (i + (width-k) - offset) % N
prior[i] += pdf[index] * kernel[k]
return prior
可以看到，卷积其实就是对所有概率做了一个模糊。它把每个位置的概率，根据卷积核分散到相邻的几个位置上。比如位置7是百分之100，卷积涂抹后就是6:10,7:80,8:10.
而我们代码中的后验概率非常简单。他直接对目前测量到的位置类型乘以一个增益系数1.5。比如目前是门，所有是门的位置就增益1.5。然后进行归一化，我们就可以看到概率向有门的地方集中。
这一章的内容非常简洁明了。
整个内容其实可以用一个公式来概括：
posterior = likelihood × prior/
normalization factor
下面我将展示示例代码：
%接下来将实现一个在一维走廊上，由是否在传感器旁边为传感器读数来确定位置的滤波器
%目前默认一直是移动的
% 地图：1表示有传感器，0表示没有
position_map = [1, 0, 0, 0, 1, 1, 0];
% 运动模型核函数
kernel = [0.1, 0.8, 0.1];
% 每次移动的步长
move_step = 1;
% 当检测到传感器时，用于增强概率的乘法因子
posterior_factor = 1.5;
% 模拟的传感器测量序列 (1 = 检测到, 0 = 未检测到)
measurements = [0, 0, 0, 1, 1,0];
test_measurements=repmat(position_map, 1, 3);
fprintf('开始滤波...\n');
final_belief = pos_filter(test_measurements, position_map, kernel, move_step, posterior_factor);
fprintf('\n滤波结束。\n');
disp('最终的概率分布 (belief):');
disp(final_belief);
[max_prob, estimated_pos] = max(final_belief);
fprintf('最可能的位置是: %d\n', estimated_pos);
fprintf('该位置的置信度是: %.2f%%\n', max_prob * 100);
% 定义一个新窗口用来放图像
figure;
bar(final_belief, 'FaceColor', '#0072BD');
title('最终位置概率分布', 'FontSize', 14);
xlabel('轨道位置', 'FontSize', 12);
ylabel('概率', 'FontSize', 12);
%修改x轴长度，在两边多加一个空格，这样看起来好一点
xlim([0 length(position_map) + 1]);
%启用网格
grid on;
%% 函数定义区域
function belief = pos_filter(measure_sequence, pos_map, knl, step, factor)
% 获取地图长度
N = length(pos_map);
% 初始化概率分布：开始时，认为所有位置的可能性均等
belief = ones(1, N) / N;
fprintf('初始概率分布:\n');
disp(belief);
% 循环处理每一次的测量
for i = 1:length(measure_sequence)
% --- (A) 测量更新步骤 ---
measure = measure_sequence(i);
if measure == 1
% --- 这是您要求的核心实现部分 ---
% 当测量值为1时，将所有“有传感器”位置的概率乘以因子
% 找到所有传感器位置的索引 (返回 true/false 向量)
sensor_indices = (pos_map == 1);
% 将这些位置的概率乘以 factor
belief(sensor_indices) = belief(sensor_indices) * factor;
else % 当 measure == 0 时 (未检测到传感器)
% 逻辑上，我们应该增强“没有传感器”位置的概率
non_sensor_indices = (pos_map == 0);
belief(non_sensor_indices) = belief(non_sensor_indices) * factor;
end
% 归一化，确保所有概率加起来等于1
belief = belief / sum(belief);
fprintf('\n第 %d 次测量 (%d) 后，更新的概率:\n', i, measure);
disp(belief);
belief = predict_move_convolution(belief, step, knl);
fprintf('第 %d 次移动后，预测的概率:\n', i);
disp(belief);
end
end
function prior = predict_move_convolution(pdf, offset, kernel)
% PREDICT_MOVE_CONVOLUTION - 使用卷积预测下一个状态的概率分布。
% 使用 conv 函数执行卷积, 'same' 选项使输出与输入等长
convolved_pdf = conv(pdf, kernel, 'same');
% 使用 circshift 处理循环边界的移动
prior = circshift(convolved_pdf, offset);
% 预测后归一化
prior = prior / sum(prior);
end
重要的数学概念
说实话，我很不想研究背后的数学原理————它们太复杂，太烧脑了。正如前文所说，我是个不擅长数学的愚钝家伙。但是要在以上的基础上推导出卡尔曼滤波，我们有一些东西是不得不了解的。比如我们中学熟悉的，方差，平均值，标准差，数学期望等。其中比较重要的是高斯分布Gaussian distributions ，or normal distributions（正态分布，同一个东西） 他们的公式如下： （未完待续） 下面展示的是一个高斯分布的概率密度函数。 （图片待补充） 需要注意的是，这并不是“概率函数”，而是概率密度函数。如图中所示的，22℃位置的概率接近0.2，可是这不代表这个点的概率就是0.2————恰恰相反，这个位置的概率接近于0。虽然这很反直觉，但是很显然，在0.2附近有多个点。我们取22.00001或者21.99999℃，它们的概率几乎都是0.2，那么取很多个这样的接近值，所有温度之和岂不是能超过1？我们相对于在无限多个点中取特定的一个，取到的概率自然是接近0。 Gemini这样解释道： “您可以把“概率密度”类比成“人口密度”。
如果我们说一个城市的人口密度是“每平方公里1万人”，这并不意味着任何一个单独的点上都住着1万人。一个点是无限小的，上面住的人数是0。 “人口密度”的意义在于，它告诉我们人口在某个区域的集中程度。密度越大的地方，在相同大小的区域内找到的人就越多。 同样地，概率密度告诉我们概率在某个值附近的集中程度：
图中22°C对应的概率密度是0.2，这代表在22°C这个点附近，概率的“浓度”最高。 18°C对应的概率密度大约是0.05，代表在18°C附近，概率的“浓度”比较低。”
所以，我们要获得真实概率，就要对想要的位置附近作个积分。这个积分范围，通常是按传感器的精度来计算的。 两个高斯分布函数相乘，依然是高斯分布。两个高斯分布相加，可以产生类似卷积的效果。下面我将使用一个数学公式网站来直观的表现这个。 (未完待续) 再重复一遍，用来表示一个概率密度函数，也就是一个高斯分布的公式如下：
可以看到我们可以仅仅用方差和数学期望，就可以表示出一个这样的钟形曲线….
而我们前面的例子中，对所有位置的概率做卷积，就需要对每个位置和相邻的位置做乘法，如果位置一多，那性能表现将是灾难性的
所以，在接下来的部分里，我会讨论高斯分布在我们的位置确认代码上有什么作用。
从高斯分布，贝叶斯公式到卡尔曼滤波
（外面的阿婆又在吵架了，唉唉） 我们知道，贝叶斯公式是这样的：P(A | B) = (P(B | A) * P(A)) / P(B) 这个不仅仅适用于单个概率，实际上，对于整个概率分布，它也是适用的。还记得我们说两个高斯分布相乘还是高斯分布吗？那么我们就可以用一个高斯分布来表示我们目前的概率，再乘以一个后验概率的高斯分布！ 正如下面图所展示的这样，我们成功用几个参数表达了数量庞大的贝叶斯概率离散滤波！！！！！
前面已经谈到，对于一个物体的位置x（所对应的概率），我们可以对其进行预测： \(\bar{x} = x * f(x)\) 其中，$\bar{x} $表示预测得到的位置，*表示卷积，$f(x)$表示的是一个预测过程函数。这个过程可以近似成某种加法。 然后进行更新： \(x=||\bar{x}*l||\) 这里将预测的位置做归一化，并且乘以一个后验概率来 如果使用高斯分布，高斯乘法如下：(打公式累死我了) $$ N(\mu, \sigma^2) = ||\text{prior} \cdot \text{likelihood}|| \
| = | N(\bar{\mu}, \bar{\sigma}^2) \cdot N(\mu_z, \sigma_z^2) | \ |
= N\left( \frac{\bar{\sigma}^2\mu_z + \sigma_z^2\bar{\mu}}{\bar{\sigma}^2 + \sigma_z^2}, \frac{\bar{\sigma}^2\sigma_z^2}{\bar{\sigma}^2 + \sigma_z^2} \right)
\(假设预测平均值是x，方差是y，似然平均值是a，方差是b，那么就有：\) (\frac{(ya+bx)}{y+b},\frac{yb}{y+b}) $$ 这是一个显然的乘法。如果把这个乘法扩张到多维，就变成了矩阵乘法，比如方差$\sigma^2$会变成矩阵P，P是一个方阵，对角线上的元素是各个状态量的方差（不确定性），非对角线元素是不同状态量之间的协方差（相关性）
