卡爾曼濾波及實現
參考視訊 https://www.bilibili.com/video/av4356232/
參考部落格 https://blog.csdn.net/codesamer/article/details/81191487
目錄
1.預測值 與 狀態預測公式
假設有一個小車在行駛，它的狀態是，包括它的 位置p 和 速度v，加速度u
可知其上一時刻的狀態為
發現其輸出變數是輸入變數的線性組合
轉化成矩陣的形式為
令 F 為狀態轉移矩陣（從上一時刻的狀態推測這個時刻的狀態），B 為控制矩陣（控制量u如何作用當前狀態）
得到狀態預測公式
1.1預測狀態的協方差P
使用協方差矩陣來表示 預測的值 的不確定性
假設 x 、y 兩個維度的隨機變數相互獨立，其協方差為0
假設 x 、y 兩個維度的隨機變數具有相關性，x 隨著 y 增大 協方差>0 。x 隨著 y 減小 協方差<0
協方差矩陣表示為 cov（x，x） 對角線表示兩個維度的 隨機變數的 方差。斜對角線的值相等，表示協方差
根據： 
不確定性 在各個時刻的傳遞關係（預測狀態的協方差）
加一個 協方差Q 表示預測模型本身帶來的噪聲
1.2觀測值 與 觀測噪聲協方差R
一個觀測量 Z 可以通過車輛當前的狀態 x · 觀測矩陣H 和一個誤差組成
定義觀測噪聲的協方差矩陣為 R
2.1狀態更新
有第一步等到帶 - 號的預測值，通過觀測量修正 x 的值，就可以得到一個最佳估計值
括號裡面代表實際的觀測值 和 預期的觀測值 的殘差。
K 是卡爾曼係數
作用1：
權衡預測狀態協方差 P 和 觀測噪聲協方差 R 的大小。來決定相信預測多一些（K小） or 相信觀察多一些（K大）
作用2：
把殘差的表現形式 從觀察域 轉化到 狀態域
2.2預測值的噪聲分佈（協方差）P 的更新
更新 最佳預測值的噪聲分佈 未了 下一輪迭代
在不確定性變化（噪聲）中尋找一個平衡
整合
帶 - 的值是預測的值，但是缺少觀測值的修正。所以加入觀測值後，對每一時刻的狀態進行預測。
公式1： X(k|k-1) = AX(k-1 | k-1) + BU(k) + W(k)
公式2： P(k|k-1) = AP(k-1|k-1)A' + Q(k)
公式3： X(k|k) = X(k|k-1) + Kg(k)[Z(k) - HX(k|k-1)
公式4： Kg(k) = P(k|k-1)H'/{HP(k|k-1)H' + R}
公式5 ： P(k|k) = (1- Kg(k) H) P(k|k-1)
實現
# -*- coding: utf-8 -*-
"""
Created on Tue Jan 1 15:54:48 2019
@author: aligo
"""
import numpy as np
import matplotlib.pyplot as plt
# 觀測值，1-100 每秒一部，就是速度為 1 的勻速行駛
z = [i for i in range(100)]
z_watch = np.mat(z)
# 建立一個方差為1的高斯噪聲，精確到小數點後兩位
noise = np.round(np.random.normal(0, 1, 100), 2)
noise_mat = np.mat(noise)
# 將z的觀測值和噪聲相加
z_mat = z_watch + noise_mat
#print(z_watch)
# 定義x的初始狀態
x_mat = np.mat([[0,], [0,]])
# 定義初始狀態協方差矩陣
p_mat = np.mat([[1, 0], [0, 1]])
# 定義狀態轉移矩陣，因為每秒鐘採一次樣，所以delta_t = 1
f_mat = np.mat([[1, 1], [0, 1]])
# 定義狀態轉移協方差矩陣，這裡我們把協方差設定的很小，因為覺得狀態轉移矩陣準確度高
q_mat = np.mat([[0.0001, 0], [0, 0.0001]])
# 定義觀測矩陣
h_mat = np.mat([1, 0])
# 定義觀測噪聲協方差
r_mat = np.mat([1])
for i in range(100):
x_predict = f_mat * x_mat
p_predict = f_mat * p_mat * f_mat.T + q_mat
kalman = p_predict * h_mat.T / (h_mat * p_predict * h_mat.T + r_mat)
x_mat = x_predict + kalman *(z_mat[0, i] - h_mat * x_predict)
p_mat = (np.eye(2) - kalman * h_mat) * p_predict
plt.xlabel("p")
plt.ylabel("v")
plt.plot(x_mat[0, 0], x_mat[1, 0], 'ro', markersize = 1)
plt.show()
橫座標表示離初始位置的距離 p，縱座標表示在該位置的速度 v。
初始為0，在經過一段很短的時間後，速度預測值 與 實際值1 就很接近了