以前学过，但是学的很差劲，这次借着内部培训的机会，再巩固一下。
Kallman Filter网上的教程也很多，真的，说到点子上的几个。
最近再接触卡尔曼滤波是因为一个声源定位库，Open embeddeD Audition System，它里面有两个部分，一个是定位，一个是跟踪。定位用的是GCC-PHAT算法。跟踪则用的卡尔曼滤波。
Frontiers | ODAS: Open embeddeD Audition System
GitHub - introlab/odas: ODAS: Open embeddeD Audition System
在他的论文里，还比较了粒子滤波和卡尔曼滤波。
To deal with static and moving sound sources, ManyEars also relies on a particle filter to model the dynamics of each source Grondin et al. (2013). The particles of each filter are associated to three possible states: 1) static position, 2) moving with a constant velocity, 3) accelerating. This approach however involves a significant amount of computations, as the filter is usually made of 1,000 particles, and each of them needs to be individually updated. Briere et al. (2008) proposed to reduce the number of particles by half to bring down the computational load. Experiments however demonstrated that this impacts accuracy when tracking two moving sound sources. Instead ODAS uses Kalman filter for each tracked source, as illustrated by Figure 5. Results demonstrate similar tracking performance, with a significant reduction in computational load on a Raspberry Pi 3 device (e.g., by a factor of 30, from a single core usage of 24% down to 0.8% when tracking one source, and by a factor of 14, from a single core usage of 98% down to 7% when tracking four sources Grondin and Michaud (2019).
比起来，卡尔曼比基于蒙特卡洛的粒子滤波好不是一点点。
我们首先要问，滤波要解决什么问题？其实现在很多设备都是存在误差，这个也叫噪音，滤波就是消除这些误差。
先举一个例子吧，小帅体重70，在一个有误差的秤（误差正态分布）上面秤，如何测出他的准确体重。每次测出来体重是68，69，72，73，71。
正态分布也就是高斯分布长这样：
答案很简单：
( 68 + 69 + 72 + 73 + 71) / 5 = 70.6
可以看出，由于高斯分布的特点，测试的次数越多，越准确。算法很简单，但是在计算机领域，这个算法有一个很大的缺陷。每次计算都要重头开始算，假如1000以后，一次计算就要做几乎1000次加法，这种负担太大，到后面系统只有崩的。
所以算法出现改良，引入权重：
初始估计小帅重70。
第一次测小帅68，此时权重为1。体重是70+1*（68-70）=68
第二次测69，此时权重是1/2。体重是68+1/2*(69-68)=68.5
第三次测72，此时权重1/3。体重是68.5+1/3*(72-68.5)=69.66
第四次测73，此时权重1/4。体重是69.66+1/4*(73-69.66)=70.495
第五次测71，此时权重1/5。体重是70.495+1/5*(71-70.495)=70.596
可以看出，这种算法，每次计算只依赖上一次的结果，计算时间基本上是一个常亮。这样就可以一直计算下去了。
对了，这个算法就叫做：Alpha-Beta Filter。阿尔法贝塔滤波。
例子2：
有一栋大楼
这个楼高50米。 高度计误差（标准差）是5米。 十次测量的值分别是：49.03m, 48.44m, 55.21m, 49.98m, 50.6m, 52.61m, 45.87m, 42.64m, 48.26m, 55.84m。 楼下的靓仔可以目测，误差（标准差）大约是15米。 准确高度是多少？
初始预测：目测60米，眼的估计误差（标准差）大约是15米，故方差是255。
第一次测量：49.03m，高度计的标准差是5，方差就是25，故测量方差是25。
第一次更新：计算权重 ：225/(225+25) = 0.9，预计状态：60+0.9(49.03−60)=50.13米，更新估计方差：(1−0.9)*225=22.5。
第二次预测：50.13m，新的方差22.5
第二次测量：48.44m，方差25
第二次更新：计算权重 ：22.5/(22.5+25) = 0.47，预计状态：50.13+0.47(48.44−50.13)=49.33米，更新估计方差：(1−0.47)*22.5=11.84
。。。中间省略。。。
整个卡尔曼过程如下：
用到的方程组如下：
最后结果如下图：
白话解释：有一个传感器，一个模型。 哪个更准，权重就大。 更新模型。 循环。。。
这里还是以楼的高度，根据公式做一下推导：
预测：
X` = FX + u
预测高度：在楼的例子中，因为是1维，而且楼的高度不会变化，所以F是1，X`=60
预测协方差矩阵：P1|0=AP0AT+Q=P0+Q=15*15+5*5=250米
更新：
计算卡尔曼增益：
import numpy as np
def kalman_filter(z, x, P, Q, R):
# 预测步骤
x_pred = x
P_pred = P + Q
# 更新步骤
K = P_pred / (P_pred + R)
x = x_pred + K * (z - x_pred)
P = (1 - K) * P_pred
return x, P
# 测试数据
measurements = [50.1, 50.2, 50.3, 50.4, 50.5]
initial_estimate = 50 # 初始状态估计值
initial_variance = 1 # 初始状态估计的方差
process_variance = 0.1 # 过程噪声方差
measurement_variance = 0.1 # 测量噪声方差
# 初始化状态估计和方差
x = initial_estimate
P = initial_variance
# 过程噪声方差矩阵 Q
Q = process_variance
# 测量噪声方差矩阵 R
R = measurement_variance
# 使用卡尔曼滤波进行估计
for measurement in measurements:
x, P = kalman_filter(measurement, x, P, Q, R)
print("估计位置:", round(x, 3), "方差:", round(P, 3))
在实际学习卡尔曼中间其实有几个知识点，之前搞得我有点蒙。
1 全概率和贝叶斯
一般来说预测阶段使用的全概率，更新阶段用的贝叶斯。不过这个部分我还要总结一下。
2 矩阵状态
这部分我之前确实没学过。就是速度位置全部用的状态转移矩阵来计算的。空了再总结一下。
参考：
卡尔曼滤波基础知识准备-期望，方差，协方差，协方差矩阵_卡尔曼滤波协方差矩阵的意义-CSDN博客
288
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
