kf 和 ekf 和 ukf 入门

KF 的假设：

1. 高斯分布的xx预测后的不确定性仍服从高斯分布
2. 高斯分布的xx变换到测量空间后不确定性仍服从高斯分布;
3. 1 和 2 所代表的F 、H 矩阵，线性变换应用在线性系统；
4. 线性离散系统；
5. 虽然实际生活中大多都是非线性的，比如移动机器人，因为x,y，受yaw的影响；

Kalman Filter For Dummies

http://bilgin.esme.org/BitsAndBytes/KalmanFilterforDummies?from=timeline

有一句这样描述kalman gain :

Kalman filter finds the most optimum averaging factor for each consequent state. 
Also somehow remembers a little bit about the past states.

卡尔曼滤波的本质是参数化的贝叶斯模型，通过对下一时刻系统的初步状态估计
（即状态的先验估计）以及测量得出的反馈相结合，
最终得到改时刻较为准确的的状态估计（即状态的后验估计），其核心思想即为预测+测量反馈，
而这两者是通过一个变化的权值相联系使得最后的状态后验估计无限逼近系统准确的状态真值，
这个权值即为大名鼎鼎的卡尔曼增益。

森林里的小萝卜（这一段推导，简直完美）

http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/?from=timeline

开始的例子是恒定速度前进，且不考虑白噪声的情况下。

注意： 矩阵F(k) 是 系统状态 变换 的物理意义，从x(k-1) 预测 x(K) 状态，用F(K ) 线性变换得到！
k 时刻预测 x(k）和 更新 covarance matrix P(x) 的方法，cov(Ax) = A * M * A ’

第二个例子，考虑了控制变量u(k) 和 白噪声

注意： 系统状态方程 x(k) ^ 非常直观的介绍了 系统状态变换 矩阵F(k) ， 控制输入矩阵B(k) ，控制向量u(k)

第三部分，进阶，引入噪声 Q (k) R(k)
注意 Q（过程） R（观测） 是协方差矩阵， 且这个被加到了P(k) 上。

第四部分，用观测值 来优化 估计
注意： 观测值，观测系统的矩阵 H(k)
预测值和观测值都有一个高斯分布的区间，二者的重叠区域居然还是一个高斯分布区间，且认为在融合分布的均值处，可能性最高！！！
融合分布的均值和方差，可通过前两个分布获得！

第五部分，获得的融合高斯分布，引出高斯增益 K
注意： K 是个矩阵。
作用：1 权衡预测状态协方差矩阵P 和 观察模型协方差矩阵R的大小， 决定 更相信哪个模型；
2. 将观察值的残差，转化到 ，系统状态值 的空间，（可能2者的维度不同）

示例2：
黄色小汽车（KF，就当下面的F为A, 统一符号）

https://www.jianshu.com/p/1c61aab11ca4?utm_campaign=maleskine&utm_content=note&utm_medium=seo_notes&utm_source=recommendation

同内容视频解释：

https://www.iqiyi.com/w_19rujtruhp.html

还是老外的分享，文章链接：

https://www.cl.cam.ac.uk/~rmf25/papers/Understanding%20the%20Basis%20of%20the%20Kalman%20Filter.pdf

matlab 代码

Z=(1:100); %观测值，小汽车按照1m/s速度前进；
noise=randn(1,100); % 方差为1的噪声
Z=Z+noise; %赋予观察值噪声

X=[0; 0];  % 列向量，p v 的初始值；不重要
P=[1 0; 0 1];  % 协方差矩阵的初始值；不重要
F=[1 1; 0 1];  % 状态转移矩阵，第二个速度1 代表每秒采样1次，
Q=[0.0001, 0; 0 0.0001];    % 状态转移协方差矩阵，过程协方差矩阵，值小 代表对F 特别信任
H=[1 0];                    %观测矩阵，第一个1 是因为状态量第一个量为p
R=1;                        %观测噪声方差矩阵，如果有多个源的，是个一列向量

figure;
hold on;

for i=1:100
X_ = F*X;                    % X_ 减号代表状态估计量
P_ = F*P*F'+Q;
K = P_*H'/(H*P_*H'+R);
X = X_+ K*(Z(i)-H*X_);
P = (eye(2)-K*H)*P_;

plot(X(1), X(2));
end

---

---

---

EKF extended Kalman Filter
Bucy，Sunahara
如果你的状态方程和观测方程不是线性的，而是非线性的，怎么办！
那就把状态方程和观测方程线性化(EKF 和 UKF的诞生)，然后再用kf
如何线性化，泰勒级数展开，一般只使用一阶展开式，
但是忽律了高阶展开式，带来较大误差，当系统状态方程为强非线性时，可能会使滤波发散；
但是如果采用二阶展开式，又会使计算量增大，难以工程实现，还是一阶使用广泛

将f() 和 h() 在x(k) 附近展开成泰勒级数

Jacobian
对于值为标量的多变量的函数 f(x)，我们使用梯度，但是如果是值为向量的多变量的函数怎么办呢，雅克比矩阵实际上是对于梯度的一种泛化。

别人matlab 例子

https://blog.csdn.net/lizilpl/article/details/45289541

https://blog.csdn.net/young_gy/article/details/78468153

---

---

---

UKF
Unscented Kalman Filter (无损卡尔曼滤波) ，是无损变换（UT）和KF的结合体，通过无损变换 使得 非线性系统变换成线性系统。

相比ekf ，精度更高，且省略了雅克比矩阵的计算。

选取2n+1 个signa 点，n为随机变量维数；