在OpenCV / C ++中通過（擴展）卡爾曼濾波器實現數據融合

Question

我正在研究一個項目，通過數據融合跟蹤安裝在移動設備上的攝像機的位置。 我得到的數據是

1）來自源A的攝像機的x，y和z方向的速度

2）來自B的當前和最后一幀（2D，Z不應該改變任何一種方式）中的位置之間的差異

我已經做了一個類似的項目，但沒有任何數據融合，並且使用了在OpenCV中實現的Kalman-Filter。

目前，我一直對我在網上找到的所有不同的實現技術感到困惑。

我如何將我獲得的數據插入/組合到KF / EKF的不同組件中？

我有這個例子來改變OpenCV-KF作為EKF。 它看起來很像我需要的東西，除了我的對象實際上是相機本身，還能夠在y軸和x軸上移動，輪流，....而且我沒有得到對象的像素坐標上述數據。 https://sites.google.com/site/timecontroll/home/extended-kalman-filtering-with-opencv

不幸的是，我不確定如何定義我的過渡和測量功能以及相應的矩陣。

設x（k）為當前狀態，deltaT是自上次更新以來經過的時間。

對於1它可能是這樣的：

x（k）= x（k-1）+ deltaT乘以速度（k-1）

2：

x（k）= x（k-1）+ differenceBetweenFrames

所以讓我們假設兩個數據源的權重相同，這讓我覺得結合它會是這樣的：

x（k）= x（k-1）+（deltaT乘以速度（k-1）+ differenceBetweenFrames）/ 2

對於測量：我通過源A獲得X，Y和Z方向的速度以及通過源B獲得X和Y坐標中當前和最后一幀的差異。

那是否有意義，或者我對那里的任何事情都已經錯了？ 我如何調整轉換和測量矩陣以及相應地更新當前狀態？

Answer 1

您處於正確的軌道上，但您似乎對轉換功能和測量功能的概念感到困惑。

首先要做的是明確定義要考慮的狀態向量。 在您的情況下，位置[px，py，pz]和velocity [vx，vy，vz]就足夠了：

x _k = [px _k ，py _k ，pz _k ，vx _k ，vy _k ，vz _k ]

第二件事是選擇轉換函數來模擬系統的動態。 請注意，這與傳感器測量無關：它只是系統狀態隨時間演變的模型。 在位置和速度的情況下，一個非常簡單的模型如下（但如果需要，您可以選擇使用更高級的模型）：

p _k = [px _k ，py _k ，pz _k ] = p _k-1 + dT。 v _k-1

v _k = v _k-1

這可以按矩陣形式放置如下：

       [ 1  0  0  dT 0  0  ]
       [ 0  1  0  0  dT 0  ]
       [ 0  0  1  0  0  dT ]
x(k) = [ 0  0  0  1  0  0  ] . x(k-1)
       [ 0  0  0  0  1  0  ]
       [ 0  0  0  0  0  1  ]

最后，您需要為傳感器推導出測量功能。 該測量函數表示當我們知道狀態向量時可以推斷傳感器測量的方式。

您的源A測量系統的速度，因此測量功能如下：

h _A （ x _k ）= v _k

或者以矩陣形式：

           [ 0 0 0 1 0 0 ]
hA{x(k)} = [ 0 0 0 0 1 0 ] . x(k)
           [ 0 0 0 0 0 1 ]

您的源B測量位置差異，因此測量功能如下：

h _B （ x _k ）= p _k -p _k-1 = p _k-1 + dT。 v _k-1 - p _k-1 = dT。 v _k-1 = dT。 v _k

或者以矩陣形式：

             [ 0  0  0  dT 0  0  ]
hB{ x(k) } = [ 0  0  0  0  dT 0  ] . x(k)
             [ 0  0  0  0  0  dT ]

---

如果要為源B提供更准確的測量功能，可能需要在狀態向量中添加上一時間步的位置，如下所示：

x _k = [px _k ，py _k ，pz _k ，px _k-1 ，py _k-1 ，pz _k-1 ，vx _k ，vy _k ，vz _k ]

過渡功能將變為：

       [ 1  0  0  0  0  0  dT 0  0  ]
       [ 0  1  0  0  0  0  0  dT 0  ]
       [ 0  0  1  0  0  0  0  0  dT ]
x(k) = [ 0  0  0  1  0  0  0  0  0  ] . x(k-1)
       [ 0  0  0  0  1  0  0  0  0  ]
       [ 0  0  0  0  0  1  0  0  0  ]
       [ 0  0  0  0  0  0  1  0  0  ]
       [ 0  0  0  0  0  0  0  1  0  ]
       [ 0  0  0  0  0  0  0  0  1  ]

源A的測量功能將變為：

           [ 0 0 0 0 0 0 1 0 0 ]
hA{x(k)} = [ 0 0 0 0 0 0 0 1 0 ] . x(k)
           [ 0 0 0 0 0 0 0 0 1 ]

源B的測量功能將變為：

           [ 1  0  0 -1  0  0  0  0  0 ]
hB{x(k)} = [ 0  1  0  0 -1  0  0  0  0 ] . x(k)
           [ 0  0  1  0  0 -1  0  0  0 ]

---

話雖這么說，測量系統的速度和它的位置差異幾乎是相同的。 由於你從不測量位置，你的狀態可能不會被觀察到，這可能是一個問題。 有關更多詳細信息，請查看以下文檔：

• F.Haugen的“卡爾曼濾波器狀態估計”（ pdf ）
• Southall，Buxton和Marchant的“可控性和可觀測性：卡爾曼濾波器設計工具”（ pdf ）