卡爾曼濾波的初步理解

這篇博客寫的很好，主要的理解過程是從他這裡得到的，寫的都是矩陣運算，慢慢看挺好懂的，重點是裡面對高斯分布的乘積的理解就是對卡爾曼濾波來源的理解：

輕鬆理解卡爾曼濾波

這篇文章寫了一個比較簡單的python代碼，從代碼這裡又獲得了進一步的理解：

https://blog.csdn.net/codesamer/article/details/81191487

以下是我對卡爾曼濾波理解過程中做的一些思考和筆記：

涉及到的各個參數及其含義：

xt t 時刻的狀態，預測值，估計值

zt t 時刻的觀察值

Q，R Q 預測模型噪音的協方差矩陣，R 觀測噪音的協方差矩陣；

H 當前狀態和當前觀測值的關係矩陣，用它可以從當前狀態推導出當前觀測值，但是這是推導出的，不是實際觀測到的嗎？？？乘以 H 後才最終完成了預測值

K 卡爾曼增益權重！

P 預測值的距離和速度之間的誤差協方差矩陣？表示預測的不確定性；雜訊協方差矩陣對系統會產生影響，所以我們要傳遞到系統中去

Fk 狀態轉移矩陣（比如速度，距離轉移矩陣）

Bk,Uk 狀態控制矩陣 , 狀態控制向量 (比如加速度狀態轉移矩陣，加速度) ；一般沒有用上

不斷迭代的系統變數只有 3 個，分別是系統的狀態 Xt ，其誤差協方差矩陣 P ，和卡爾曼增益 K ;

Q，R 和 P的區別是什麼？

Q 是預測模型的誤差，是狀態轉移的過程中加入的，比如用勻速模型或者勻加速模型去預測物體狀態造成的誤差；

R 是觀測測量的誤差，這是感測器的特性決定的，很好理解；

P 是上一個時刻估計出來狀態的誤差；從它的更新公式可以看出來，它是由最初的 P 和 R 一起迭代生成的，所以它實際上就是上一個時刻的綜合的誤差。

誤差分為兩大塊，預測結果和觀測結果的誤差，P 和 Q 合起來才是最終預測值的誤差， R 單獨就是觀測誤差；

H 的作用是什麼？

對觀測值做一個形式的變化，不改變實質；

由於我們的狀態變數是一個二維矩陣，而表示距離的是一個標量，所以狀態變數矩陣就要乘以一個觀測矩陣H，當然我們觀測到的位置也不一定是絕對準確的，所以也要加上一個觀測雜訊

核心的思想是什麼？

1 用加權平均的觀測值和預測值表示真實值（真實值始終是未知的，只能接近）

2 K 的來源是兩個誤差高斯概率密度函數的乘積（最後公式使用預測結果和觀測結果的方差，也就是 P 和 R )；大概表達成公式就是 K=P/(P+R)

3 理解多個誤差分布才是理解卡爾曼濾波的核心！

權重的調整怎麼進行的？

K 的核心思想表達成大概的公式是 K=P/(P+R) ,那麼如果上一時刻的 P 過大，這次得到的權重就會比較大，從而在加權計算中更加偏向於觀測值，反之亦然；

K=P/(P+R) 怎麼來的看第一個博客鏈接；

不存在真值，也沒有和真值相減的過程，怎麼知道 P 的大小是否合理呢？

待解答。

我對第二個鏈接代碼做的進一步注釋：

# coding:utf-8 # 卡爾曼濾波理解代碼 # 模擬預測小車位置和速度的一段程序， # 觀測的值是一段從0到99，速度為1的勻速行駛路徑， # 加入的雜訊是均值為0，方差為1的高斯雜訊

import numpy as np
import matplotlib.pyplot as plt

# x 初始狀態,表示離原點距離和速度
# 為什麼這裡有2個值，zmat 又每次只有1個值？
x_mat = np.mat([[0,],[0]])

# 觀測值Z
z = np.arange(100)
# 值得注意的是它每次只有一個值，表示距離，只觀測當前距離
z_watch = np.mat(z)

# 觀測噪音 R ,是一個 N（0,1）分布的隨機數
R_noise = np.random.normal(0,1,100)
R_noise_mat = np.mat(R_noise)

# 觀測值Z + 觀測噪音R
z_mat = z_watch + R_noise_mat

# 初始狀態協方差矩陣 P，開始是全然不相關的；記住它是綜合誤差
p_mat = np.mat([[1,0],[0,1]])

# 狀態轉移矩陣F，
f_mat = np.mat([[1,1],[0,1]])

# 狀態轉移協方差矩陣 Q, 協方差比較小，
# 說明狀態轉移矩陣比較正確，預測模型比較準確；
q_mat = np.mat([[0.0001,0],[0,0.0001]])

# 觀測轉換矩陣 H，shape是1*2；把它和x_predict（2*1）相乘，才能轉換得到 1 個的距離值
h_mat = np.mat([1,0]) # 1，0 表示只取第一個

# 觀測噪音協方差 R ,為1 表示沒有噪音？不是，表示觀測誤差方差恆定為1；
# 因為我們觀測值的分布就是（0,1）分布；
r_mat = np.mat([1])

for i in range(100):
x_predict = f_mat * x_mat
print(x_predict,x_predict)
p_predict = f_mat * p_mat * f_mat.T + q_mat
print(p_predict,p_predict) # 預測值最終誤差，左下角和右下角元素從大變小，因為開始速度準確性比較低，後面比較準確了

kalman = p_predict * h_mat.T / (h_mat * p_predict * h_mat.T + r_mat)
print(kalman,kalman)
# kalman 的第二個值也是一開始比較大，說明比較偏向於速度觀測值，後續變小
x_mat = x_predict + kalman*(z_mat[0,i] - h_mat * x_predict)
print(z_mat[0,i] - h_mat * x_predict,z_mat[0,i] - h_mat * x_predict) # 這一步只得到了一個值的結果
print(kalman*(z_mat[0,i] - h_mat * x_predict),kalman*(z_mat[0,i] - h_mat * x_predict))
p_mat = (np.eye(2) - kalman * h_mat) * p_predict
print(p_mat,p_mat)
# p_mat 的協方差的左下角和右下角元素從大變小，因為開始速度準確性比較低，後面比較準確了