卡尔曼滤波（二）

数据融合

有两个测量设备，分别有：

测量值：Z₁ = 30 ，测量误差 σ₁ = 2 

测量值：Z₂  = 32，测量误差 σ₂ = 4 

服从正态分布：

 

 如果将设备编号1、2视为测量次数，那么：

按照卡尔曼滤波的算法：

估计值 Z^{^}  = Z₁ + K（Z₂ - Z₁），K：卡尔曼增益

K∈ [ 0, 1 ]（K在0~1之间）

若：

1. K = 0 ， Z^{^}  = Z₁ ，（换句话说，没有增益，那么就只相信上一次测的）

2. K  = 1， Z^{^}  = Z₂ ，（换句话说，只相信这次测的）

* K就很像一个概率，或者相信度之类的东西。

---

 

如何求K？

K有一个原则，要使得【估计值误差，或叫标准差（ σ_Z^）】 最小。

σ_Z^最小 ，等价于 Var(Z^) , Var()就是求方差的函数。 (σ_Z^)² = Var(Z^)

Var(Z^) = Var(Z₁ + K（Z₂ - Z₁）)

 

 相互独立，使用误差传播：

 

 要使得【估计值误差，或叫标准差（ σ_Z^）】 最小，那么就是对K求导，使得导数为0得极值：

代入，得：

Z^{^}  = Z₁ + K（Z₂ - Z₁）= 30 .4 

(σ_Z^)² = (1 - k )² * σ_1² + k² * σ₂²  = 3.2

画图：

 结论：

1. 按照上一篇：卡尔曼滤波（一） - 耀礼士多德 - 博客园 (cnblogs.com)

    K的初始值，可以在测量到第二个数据的时候，得到。

2. 经过计算 Z^{^} ，发现的Z^{^} 误差σ_Z^，都要较前两次测量的要小。

---

 

两条公式放在一起看：

1. 

 

2. 

 

 模型误差的初始值、经验值是不是可以用测量误差近似估计？？？

---

 

协方差矩阵

普通版计算协方差

 

 

矩阵版计算协方差

---

卡尔曼滤波入门

理论上：

X_k=AX_k-1+BU_k+W_{k-1 ......}（1）

Z_k=HX_k+V_k   ......（2）

（之所以叫理论上，就像三角形的内角和理论上等于180°，实际上测量值如果代入理论公式，那么等号不成立 ）

以房间温度为研究对象。

X_k：是k时刻的系统状态。  在此处，为第k时刻的房间温度。（当k=0时，X_k 可以看做凭经验瞎猜的温度）

A：是系统参数、【状态转移矩阵】。假设温度是随着时间变化的，那么A矩阵中，就有K的存在（可能叫A_k比较合适吧？）；如果温度是恒定的，那么A就是1，或单位矩阵。

U_k : 是k时刻对系统的控制量、【控制矩阵】。可不可以理解为加常数？类似于光照对温度的影响之类？由于系统中一般不太有控制量，所以B这个参数一般为0，也就是没有U(K)。

W_k：过程噪声。被假设成高斯白噪声。协方差分别是【Q】。（暂时无法理解过程噪声）

Z_k：是k时刻的测量值。在此处，为第k时刻，用温度计测量房间的温度值。（Z_k是有可能是多种测量值，写为列矩阵）

H：是测量系统的系数矩阵。在此处，因为Z_k 和X_k都是温度，那么H=1。如果X_k 假设是汽车的里程，而Z_k是轮胎圈数，那么H就有一个转换关系了。

V_k ：是测量误差。协方差为【R】。

以小车运动为研究对象。 

X_t = [P_t，V_t]^T

P_t：小车的里程。

V_t：小车的速度。

在第K时刻，小车的状态：

P_t = P_t-1 + V_t-1 * Δ_t +  1 / 2 * U_t * Δt²   .......U_t为t时刻的加速度

V_t = V_t-1  + U_t * Δt

(暂时忽略W_k)

形如：X_k=AX_k-1+BU_k

 以房间温度为研究对象：

卡尔曼滤波，就是以X_t-1（Xt-1已经是优化过的，不是单纯从X0直接按式子去推，稍后再看），去预测X_t。

1. 假设屋子里的温度是恒定的。解读：那么A就变为单位矩阵，也就是，理论上：X_t = X_t-1 + W_t。(W_t 更多是一个象征意义，在计算的时候更加关注W_t 的协方差Q)

2. 假设温度X₀（或者X_t-1）为23，假设噪声为3。

（t-1时刻，【最优温度偏差p_(k-1|k-1) 】= 3²，协方差矩阵）

4. 现在，假设 t = 2，X_{( t |t -1)=} X_t + BU_t = 23 ，假设噪声为4，也就是Q = 4²。

(X_{( t |t -1)}代表由 t -1 预测 t 时刻的值，之所以不用X_t，代表这个值还不是最优预测，否则就用X_t 或者【X_(t|t)】代表【最优估计】，不加W_t 是因为它只是一个象征，不可知，只能知道其噪声)

5. 计算 P_(k|k-1)=A*P_(k-1|k-1)A' + Q ，在此处为 3*3 + 4*4 = 5*5。 

(P_(t|t-1)代表由 t -1 预测 t 时刻的 温度偏差，因为还不是最优的，否则就用否则就用P_t 或者P_(t|t)代表【最优温度偏差】)

6. 然后，在温度计那里，读取温度值 Z_k = 25，同时R = 4² 

（相当于厂家标称温度计误差为）

7. 计算【卡尔曼增益】K_k²=P_(k|k-1) * H' / ( H * P_(k|k-1) * H ' + R ) ，此处：K_k ²= 5^2 / ( 5^2 + 4^2 )

8. 计算【最优估计】X_(k|k) = X_(k|k-1) + K_k* (Z_k -  H * X _(k|k-1) )，此处：23 + 0.78 * (25-23)=24.56

9. 计算【最优偏差】P_(k|k)=( I - K_k *H ) * P_(k|k-1)，留给下一次测量时用

---