滤波器
1 随机状态和估计
随机过程：stochastic process
随机过程 = 时间 + 不确定性（期望、方差）：概率
随机过程：
例如 IMU 测重力加速度，事件为X(t)，读数为w，则随机过程就是X(t，w)，时间和读数的关系
加速度的变化：
随时间变化------------>随机过程
随机变量：
例如正态分布：
概率：
研究随机事件的数学统计特性 P(X)
条件概率：
机器人的初始位置：P(B)
下一步：P(X|B)
P(X) = P(X|B) × P(B)
假设前面已经走了n步：{X1,X2,…Xn}：
全概率：
P(X) = P(X|X1,X2,…Xn) × P(B) × P(X1) × P(X2) × P(…)× P(Xn)
因为有地图的存在，所以有观测P(Z)
P(Z|X)
目的： 估计状态P(X)
控制量使机器人作出预测： P(X) = P(X|B) × P(B)
预测的同时存在观测： P(Z|X,B)
滤波器研究问题的核心：P(当前状态|控制，观测)
贝叶斯：
P(X|B,Z) = P(Z|X,B)P(X|B) / P(Z|B)
2 卡尔曼滤波器(KF)：
控制量使机器人作出预测：P(X) = P(X|B) × P(B)
预测的同时存在观测：P(Z|X,B)
卡尔曼滤波器假设：量测的噪声是高斯白噪声（均值为0）
P(X) = P(X|B) × P(B) ——> X = 期望+不确定性
P(Z|X,B) ——> Z = 期望+不确定性: Z = HX
融合估计和观测：
观测和估计是相互独立的，所以可以直接相乘（P(AB) = P(A) × P(B)）：
融合之后的概率分布：
展开得：
Z = HX ——> δz = H × δm
卡尔曼增益：卡尔曼滤波器：融合：
Xf = Xp +K(Xm - H*Xp)
δf ² = δp ² - KHδp ²
3 扩展卡尔曼滤波器(EKF)和SLAM
非线性问题：
运动模型（Mono SLAM）：
位置投影到world frame是非线性的
旋转投影到world frame是非线性的
landmark ——> 相机平面 ——> world frame ：非线性
VINS（Mutil-State）：
quaternion、速度的映射都是非线性过程：部分非线性
非线性问题无法用线性模型（卡尔曼滤波器）解决
做偏导（非线性依赖项很多，耦合强，所以矩阵对依赖项做偏导）：
| 特性 | 方法 |
|---|---|
| 非线性或部分非线性 | EKF |
| 线性 | KF |
视惯融合：
已知：相机位姿（x,y,z,α,β,γ），landmark位置（x,y,z）（相机坐标系下）
通过针孔相机模型，逆深度求解（将深度信息丢弃），得出landmark在像素坐标中的位置，和图像上landmark的位置的偏差为卡尔曼做误差校正
4 粒子滤波器
随着landmark的增多，矩阵变得十分庞大
假设landmark之间都是独立的：
工作过程：
随机撒点，各个粒子按照不同的方向运动（运动方向不变），粒子看到的（传感器的信息）如果和地图相符，该粒子打分高，和地图不符，打分低，被丢弃，新粒子在打分高的粒子附近再次生成，最终收敛，定位完成