卡爾曼濾波器（一維）：幾種方法？

Question

我試圖了解卡爾曼濾波器的工作原理，並且因為多維變體對於一開始來說太混亂了，所以我從一維示例開始。

我找到了 3 個不同的來源來解釋溫度計的場景，但所有這些場景都實現了略有不同的方程，我不明白這一點。

我實施了解決方案 2，但我的卡爾曼濾波器並沒有真正起作用（它高度適應測量，並沒有真正考慮其上的噪聲）。

因此，在我浪費更多時間嘗試解決方案 1 或 3（直到現在我才讀過）之前：有人可以為一維卡爾曼濾波器提供清晰的解釋和/或代碼示例嗎？

---

解決方案1

// x_est: current estimate;           p: current estimate error;
// a:     constant of the system;    kg: kalman gain
// z:     current observation; 

// Predict
x_est   =   a * x_est
p       =   a * p * a

// Update
kg      =   p  / (p  + r)
x_est   =   x_est + kg * (z - x_est)
p       =   (1 - kg) * p

作者（此處）僅說明我們僅更改當前值，因為溫度計無需考慮最后一個值。

所以他簡化了：

p[k] = (1 - kg) * p[k-1]到p = (1 - kg) * p

x_est[k] = x_est[k-1] + kg * (z - x_est[k-1])到x_est = x_est + kg * (z - x_est)

...等等...

我不明白為什么這是可能的。 我認為卡爾曼濾波器的主要部分之一是考慮當前觀察值z是否有用（通過卡爾曼增益）。 因此，對於高卡爾曼增益kg * (z - x_est[k-1]) ，將增量z - x_est[k-1]的“大塊”添加到新估計中。 如果總是計算當前值，這整件事不是變得毫無意義嗎？

---

解決方案2

# q: process variance / process noise
# r: error in measurement

x_est = x_est
p     = p + q;

k     = p / (p + r);
x_est = x_est + k * (z – x_est);
p     = (1 – k) * p;

這幾乎是一樣的，但作者甚至沒有解釋為什么x[k-1]和p[k-1]可以更改為x和p 。

---

解決方案3

# Q: process variance / process noise
# R: error in measurement

# prediction
x_est_kminus1[k] = x_est[k - 1]
p_kminus1[k]        = p[k - 1] + Q

# update
kg[k]     = p_kminus1[k] / (p_kminus1[k] + R)
x_est[k] = x_est_kminus1[k] + kg[k] * (z[k] - x_est_kminus1[k])
p[k]     = (1 - kg[k]) * p_kminus1[k]

在這個解決方案中，作者有兩個不同的列表用於x_est （ x_est本身和x_est_kminus1 ）和p （ p本身和p_kminus1 ）。

是否需要兩個列表，否則 p[k] 將被計算兩次（在預測和更新步驟中）？

Answer 1

所有這些解都是一般方程的特例，我們必須看看每個方程有什么特別之處。

適當的方程

讓我們從一維情況的適當一般方程開始：

# prediction
x[k] = a * x[k - 1]
p[k] = a * p[k - 1] * a + q
# update
y = z - h * x[k]
kg = p * h / (h * p * h + r)
x[k] = x[k] + kg * y
p[k] = (1 - kg * h) * p[k]

• x - 狀態
• p - 誤差（協方差）
• a - 狀態轉換
• q - 轉換錯誤
• z - 測量
• h - 狀態到測量的轉換
• y - 我們根據預測期望測量的值與我們實際測量的值之間的差異
• kg - 卡爾曼增益
• r - 測量誤差

模型的所有參數（ a 、 q 、 r 、 h ）原則上也可以有一個索引k並隨着系統的發展而變化。 但在簡單的情況下，它們都可以視為常數。

解與正確方程有何不同

只有解決方案 1 實現了a ，這很好。 a告訴您狀態如何從一個步驟更改為另一個步驟，如果您假設溫度是固定的，那么a == 1 ，就像在解決方案 2 和 3 中一樣。

解決方案 1 沒有q 。 q是我們可以估計過程誤差的地方。 同樣，如果該過程是關於系統靜止的（ a == 1 ），那么我們可以設置q = 0 。

您的解決方案都沒有h ，這是觀察轉換（如何從測量到狀態）。 如果您要根據溫度的測量值估計溫度，則h = 1 。

h可能與 1 不同的一個例子是，如果您測量的不是您感興趣的估計值，例如使用濕度測量值來估計溫度。 那么h將是線性變換T(humidity) = h * humidity 。 我強調線性，因為以上是線性卡爾曼濾波器方程，它們僅適用於線性（在數學意義上）系統。

當前和上一步問題

k與k - 1以及具有x_est和x_est_kminus1的問題純粹是實現問題。 在這方面，您的所有解決方案都是相同的。

您對解決方案 1 中的k和k - 1想法是錯誤的。 只有預測階段需要考慮當前和上一步（因為它是基於上一步對當前狀態的預測），而不是更新步驟。 更新步驟作用於預測。

從可讀性的角度來看，解決方案 3 最接近數學方程。 原則上，預測步驟還沒有給我們x_est[k] ，但更像是x_est[k] predicted_x_est[k] 。 然后在這個更新步驟運行predicted_x_est[k]和賦予我們的實際x_est[k]

然而正如我所說，所有的實現都是等價的，因為當它們被編程時，你可以看到在預測步驟之后，過去不再需要了。 因此，您可以安全地為p和x使用一個變量，而無需保留列表。

關於卡爾曼增益

你寫了：

因此，對於高卡爾曼增益 kg * (z - x_est[k-1])，將增量 z - x_est[k-1] 的“大塊”添加到新估計中。 如果總是計算當前值，這整件事不是變得毫無意義嗎？

在這些情況下，卡爾曼增益只能在 0 和 1 之間。什么時候最大？ 當r （測量誤差）為 0 時，這意味着我們無限信任我們的測量結果。 然后等式簡化為

x_est = x_est + z - x_est

這意味着我們丟棄我們的預測值（右側的x_est ）並將我們更新的估計值設置為等於我們的測量值。 當我們無限信任我們測量的東西時，這是一件有效的事情。

適應測量

我實施了解決方案 2，但我的卡爾曼濾波器並沒有真正起作用（它高度適應測量，並沒有真正考慮其上的噪聲）。

調整卡爾曼濾波器很棘手，需要對系統有深入的了解以及對q和r正確估計。 請記住， q是過程（狀態演化）的誤差， r是我們測量的誤差。 如果您的卡爾曼濾波器過度適應測量，則意味着：

• q太大
• r太小

或兩者的結合。 您將不得不玩弄這些值以找到有效的值。