在使用多目標跟蹤算法時，接觸到卡爾曼濾波，一直沒時間總結下，現在來填坑。。
1. 背景知識
在理解卡爾曼濾波前，有幾個概念值得考慮下：時序序列模型，濾波，線性動態系統
1. 時間序列模型
時間序列模型都可以用如下示意圖表示：
這個模型包含兩個序列，一個是黃色部分的狀態序列，用X表示，一個是綠色部分的觀測序列（又叫測量序列、證據序列、觀察序列，不同的書籍有不同的叫法，在這里統一叫觀測序列。）用Y表示。狀態序列反應了系統的真實狀態，一般不能被直接觀測，即使被直接觀測也會引進噪聲；觀測序列是通過測量得到的數據，它與狀態序列之間有規律性的聯系。
上面序列中，假設初始時間為\(t_1\), 則\(X_1,Y_1\)是\(t_1\)時刻的狀態值和觀測值，\(X_2,Y_2\)是\(t_2\)時刻的狀態值和觀測值...，即隨着時間的流逝，序列從左向右逐漸展開。
常見的時間序列模型主要包括三個：隱爾馬爾科夫模型，卡爾曼濾波，粒子濾波。
2. 濾波
時間序列模型中包括預測和濾波兩步
- 預測：指用當前和過去的數據來求取未來的數據。對應上述序列圖中，則是利用\(t_1\)時刻\(X_1,Y_1\)的值，估計\(t_2\)時刻\(X_2\)值。
- 濾波：是用當前和過去的數據來求取當前的數據。對應上述序列圖中，則是先通過上一步的預測步驟得到\(X_2\)的一個預測值，再利用\(t_2\)時刻\(Y_2\)的值對這個預測值進行糾正，得到最終的\(X_2\)估計值。（通俗講，就是通過\(X_1\)預測一個值, 通過傳感器測量一個值\(Y_2\), 將兩者進行融合得到最終的\(X_2\)值）
3.線性動態系統
卡爾曼濾波又稱為基於高斯過程的線性動態系統(Linear Dynamic System, LDS), 這里的高斯是指：狀態變量\(X_t\)和觀測變量\(Y_t\)都符合高斯分布；這里的線性是指：\(X_t\)可以通過\(X_{t-1}\)線性表示，\(Y_t\)可以通過\(X_t\)線性表示；如果用數學表達式來表達這兩層含義如下：
\[X_t = FX_{t-1} +w_{t-1}, \quad w_{t-1}\sim N(0, Q)\\ \]
上面表達式中F是一個矩陣，常稱作狀態轉移矩陣，保證了\(X_t\)和\(X_{t-1}\)的線性關系(線性代數中，矩陣就是線性變換)；\(w_{t-1}\)常稱作噪聲，其服從均值為0，方差為Q的高斯分布，保證了\(X_t\)服從高斯分布(因為高斯分布加上一個常數后依然是高斯分布)。
同樣的關於\(X_t\)和\(Y_t\),也可以得到如下表示, 其中矩陣H稱作狀態空間到觀測空間的映射矩陣， \(r_t\)稱作噪聲，其服從高斯分布：
\[Y_t = HX_t+r_t, \quad (r_t\sim N(0, R)) \]
參考：https://zhuanlan.zhihu.com/p/139215491
2. 卡爾曼濾波理論知識
關於卡爾曼濾波的理論知識有很多文章講的很好，參考下面的兩個鏈接
https://zhuanlan.zhihu.com/p/25598462
http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/#mjx-eqn-gaussequiv
這里簡單總結下用得到數學表達式，方便實際編程時查閱和理解：
2.1 狀態序列的預測
在時間序列模型圖中，這一步表示的是, 在預測序列中，從\(X_{t-1}\)到\(X_t\)的預測步驟。
需要用到的變量含義如下：
- \(\hat x_k\):狀態變量
- \(\hat P_k\): 狀態變量的協方差矩陣
- \(F_k\):狀態轉移矩陣
- \(B_k\):控制矩陣
- \(\mu_k\):控制向量
- \(w_k\): 狀態變量的噪聲矩陣
- \(Q_k\):協方差矩陣的噪聲矩陣
數學表達式如下：
\[\hat x_k = F_k\hat x_{k-1} +B_k \vec \mu_k+w_k\\ \hat P_k = F_k\hat P_{k-1}F_k^T+Q_k \]
2.2 狀態序列到觀測序列的轉換
在時間序列模型圖中，這一步表示的是, 從\(X_{t}\)到\(Y_t\)的轉換。
需要用到的變量含義如下：
- \(H_k\):狀態空間到測量空間的映射矩陣
- \(\hat x_k\): \(t_k\)時刻的狀態變量
- \(\hat P_k\): \(t_k\)時刻狀態變量的協方差矩陣
映射后變量的均值和方差如下：
\[\vec\mu_{expected} = H_k\hat x_k\\ \Sigma_{expected} = H_k\hat P_kH_k^T\\ \]
2.3 測量序列的濾波
在時間序列模型圖中，這一步表示的是，測量值\(Y_t\)對預測值\(X_t\)進行糾正，得到最終的狀態向量\(X_t\)
觀測向量服從高斯分布, 假設其均值和協方差矩陣如下：
\[觀測向量均值：\vec\mu_{measurement} = \vec z_k\\ 觀測向量協方差矩陣:\Sigma_{measurement}=R_k \]
我們已經得到了狀態序列的預測值，而且將其映射到了測量序列，也得到了測量序列的測量值，接來要做的就是將兩者進行融合，由於兩者都符合高斯分布，其融合過程就是兩個高斯分布的混合
1. 高斯混合
單變量高斯分布方程：
\[\mathcal{N}(x,\mu,\sigma) =\frac{1}{\sigma \sqrt{2\pi}}e^{-\frac{(x-\mu)^2}{2\sigma^2}}\\ \begin{equation} \mathcal{N}(x,\mu_0,\sigma_0)\cdot \mathcal{N}(x,\mu_1,\sigma_1)\overset{?}{=}\mathcal{N}(x,\mu^{'},\sigma^{'}) \end{equation}\\ \]
兩個單變量高斯分布混合：
\[\mu^{'} =\mu_0+k(\mu_1- \mu_0)\\ {\sigma^{'}}^2=k{\sigma_1}^2={\sigma_0}^2(1-k)\\ 其中，k=\frac{\sigma_0^2}{\sigma_0^2+\sigma_1^2} \]
以上是單變量概率密度函數的計算結果，如果是多變量的，那么，就變成了協方差矩陣的形式：
\[K=\Sigma_0(\Sigma_0+\Sigma_1)^{-1}\\ \overrightarrow\mu^{'}=\overrightarrow\mu_0+K(\overrightarrow\mu_1-\overrightarrow\mu_0)\\ \Sigma^{'}=K\Sigma_1=\Sigma_0(I-K) \]
2. 預測值和測量值混合
預測值和測量值的均值和協方差如下：
\[預測值高斯分布：(\mu_0,\Sigma_0)=(H_k\hat x_k,H_kP_kH_k^T)\\ 測量值高斯分布：(\mu_1,\Sigma_1)=(z_k,R_k) \]
預測值和測量值混合：
\[H_k\hat x_k^{'}=H_k\hat x_k+K(\overrightarrow{z_k}-H_k\hat x_k)\\ H_kP_k^{'}H_k^T=H_kP_kH_k^T-KH_kP_kH_k^T\\ 其中，K稱作卡爾曼增益：K=H_kP_kH_k^T(H_kP_kH_k^T+R_k)^{-1} \]
化簡上述表達式，計算\(x_k^{'},P_k^{'}\)，結果如下：(\(x_k^{'}\)就是第k次卡爾曼預測結果, \(P_k^{'}\)是該結果的協方差矩陣)
\[\hat x_k^{'}=\hat x_k+K^{'}(\overrightarrow{z_k}-H_k\hat x_k)\\ P_k^{'}=P_k-K^{'}H_kP_k\\ K^{'}=P_kH_k^T(H_kP_kH_k^T+R_k)^{-1} \]
最終，可以卡爾曼濾波流程圖如下：
參考：https://zhuanlan.zhihu.com/p/142044672
https://zhuanlan.zhihu.com/p/158751412
https://zhuanlan.zhihu.com/p/58675854
https://leijiezhang001.github.io/卡爾曼濾波詳解/
https://zhuanlan.zhihu.com/p/166342719
3. 卡爾曼濾波用於目標跟蹤
對於一個小車，我們采用卡爾曼濾波來估計其位置，在進行估計的時候，有幾個變量要進行定義和初始化。這里定義了六個變量的初始值：狀態向量，狀態向量協方差矩陣，狀態向量協方差矩陣的噪聲矩陣，動態轉移矩陣，映射矩陣，測量值得協方差矩陣（假設小車是勻速運動，所以沒有使用控制矩陣和控制變量）
1. 初始狀態向量\(x\)：（8x1的矩陣）
\[\left[ \begin{array}{ll} x& y& a& h& v_x& v_y& v_a& v_h& \\ \end{array} \right]^{T}\\ \]
其中對應含義和初始化值如下：
- (x,y)表示目標的中心點，a=w/h表示目標的寬高比，h表示目標的高度，初始化值為目標第一幀的位置
- v_x, v_y, v_a, v_h:表示對應變量隨時間的變化率, 初始化值為0
2. 狀態向量初始協方差矩陣\(P\)：(8*8的矩陣， 這里如何設置，需要經驗？)
\[\left[ \begin{array}{ll} 151.29& 0& 0& 0& 0& 0& 0& 0& \\ 0& 151.29& 0& 0& 0& 0& 0& 0& \\ 0& 0& 0.0001& 0& 0& 0& 0& 0& \\ 0& 0& 0& 151.29& 0& 0& 0& 0& \\ 0& 0& 0& 0& 59.09& 0& 0& 0& \\ 0& 0& 0& 0& 0& 59.09& 0& 0& \\ 0& 0& 0& 0& 0& 0& 0.00001& 0& \\ 0& 0& 0& 0& 0& 0& 0& 59.09& \\ \end{array} \right] \]
3.狀態向量協方差矩陣的噪聲矩陣\(Q\)：(8*8的矩陣， 這里如何設置，需要經驗？)
\[\left[ \begin{array}{ll} 37.82& 0& 0& 0& 0& 0& 0& 0& \\ 0& 37.82& 0& 0& 0& 0& 0& 0& \\ 0& 0& 0.0001& 0& 0& 0& 0& 0& \\ 0& 0& 0& 37.82& 0& 0& 0& 0& \\ 0& 0& 0& 0& 0.59& 0& 0& 0& \\ 0& 0& 0& 0& 0& 0.59& 0& 0& \\ 0& 0& 0& 0& 0& 0& 0.00001& 0& \\ 0& 0& 0& 0& 0& 0& 0& 0.59& \\ \end{array} \right] \]
4. 動態轉移矩陣F：(8x8的矩陣)
\[\left[ \begin{array}{ll} 1& 0& 0& 0& 1& 0& 0& 0& \\ 0& 1& 0& 0& 0& 1& 0& 0& \\ 0& 0& 1& 0& 0& 0& 1& 0& \\ 0& 0& 0& 1& 0& 0& 0& 1& \\ 0& 0& 0& 0& 1& 0& 0& 0& \\ 0& 0& 0& 0& 0& 1& 0& 0& \\ 0& 0& 0& 0& 0& 0& 1& 0& \\ 0& 0& 0& 0& 0& 0& 0& 1& \\ \end{array} \right] \]
5. 映射矩陣H：（4x8的矩陣）
\[\left[ \begin{array}{ll} 1& 0& 0& 0& 0& 0& 0& 0& \\ 0& 1& 0& 0& 0& 0& 0& 0& \\ 0& 0& 1& 0& 0& 0& 0& 0& \\ 0& 0& 0& 1& 0& 0& 0& 0& \\ \end{array} \right] \]
6. 測量值的協方差矩陣\(R\)：(4x4的矩陣, 這里如何設置，需要經驗？)
\[\left[ \begin{array}{ll} 37.82& 0& 0& 0& \\ 0& 37.82& 0& 0& \\ 0& 0& 0.0001& 0& \\ 0& 0& 0& 37.82& \\ \end{array} \right] \]
下面代碼中，采用卡爾曼濾波，對小車的隨機運動進行了估計，結果如圖所示，綠色框表示小車的真實位置，紅色框表示卡爾曼濾波的估計位置
# vim: expandtab:ts=4:sw=4
import numpy as np
import scipy.linalg
import cv2
"""
Table for the 0.95 quantile of the chi-square distribution with N degrees of
freedom (contains values for N=1, ..., 9). Taken from MATLAB/Octave's chi2inv
function and used as Mahalanobis gating threshold.
"""
chi2inv95 = {
1: 3.8415,
2: 5.9915,
3: 7.8147,
4: 9.4877,
5: 11.070,
6: 12.592,
7: 14.067,
8: 15.507,
9: 16.919}
class KalmanFilter(object):
"""
A simple Kalman filter for tracking bounding boxes in image space.
The 8-dimensional state space
x, y, a, h, vx, vy, va, vh
contains the bounding box center position (x, y), aspect ratio a, height h,
and their respective velocities.
Object motion follows a constant velocity model. The bounding box location
(x, y, a, h) is taken as direct observation of the state space (linear
observation model).
"""
def __init__(self):
ndim, dt = 4, 1.
# Create Kalman filter model matrices.
self._motion_mat = np.eye(2 * ndim, 2 * ndim) # 初始化動態轉移矩陣, shape(8, 8)
for i in range(ndim):
self._motion_mat[i, ndim + i] = dt
self._update_mat = np.eye(ndim, 2 * ndim) # 初始化映射矩陣，shape(4, 8)
# Motion and observation uncertainty are chosen relative to the current
# state estimate. These weights control the amount of uncertainty in
# the model. This is a bit hacky.
self._std_weight_position = 1. / 20
self._std_weight_velocity = 1. / 160
def initiate(self, measurement):
"""Create track from unassociated measurement.
Parameters
----------
measurement : ndarray
Bounding box coordinates (x, y, a, h) with center position (x, y),
aspect ratio a, and height h.
Returns
-------
(ndarray, ndarray)
Returns the mean vector (8 dimensional) and covariance matrix (8x8
dimensional) of the new track. Unobserved velocities are initialized
to 0 mean.
"""
mean_pos = measurement
mean_vel = np.zeros_like(mean_pos)
mean = np.r_[mean_pos, mean_vel] # 狀態向量，shape(1, 8)
std = [
2 * self._std_weight_position * measurement[3],
2 * self._std_weight_position * measurement[3],
1e-2,
2 * self._std_weight_position * measurement[3],
10 * self._std_weight_velocity * measurement[3],
10 * self._std_weight_velocity * measurement[3],
1e-5,
10 * self._std_weight_velocity * measurement[3]]
covariance = np.diag(np.square(std)) # 狀態向量協方差矩陣， shape(8, 8)
return mean, covariance
def predict(self, mean, covariance):
"""Run Kalman filter prediction step.
Parameters
----------
mean : ndarray
The 8 dimensional mean vector of the object state at the previous
time step.
covariance : ndarray
The 8x8 dimensional covariance matrix of the object state at the
previous time step.
Returns
-------
(ndarray, ndarray)
Returns the mean vector and covariance matrix of the predicted
state. Unobserved velocities are initialized to 0 mean.
"""
std_pos = [
self._std_weight_position * mean[3],
self._std_weight_position * mean[3],
1e-2,
self._std_weight_position * mean[3]]
std_vel = [
self._std_weight_velocity * mean[3],
self._std_weight_velocity * mean[3],
1e-5,
self._std_weight_velocity * mean[3]]
motion_cov = np.diag(np.square(np.r_[std_pos, std_vel]))
mean = np.dot(self._motion_mat, mean) # 動態轉移矩陣*狀態向量
covariance = np.linalg.multi_dot((
self._motion_mat, covariance, self._motion_mat.T)) + motion_cov # 動態轉移矩陣*狀態向量協方差矩陣 + 噪聲矩陣
return mean, covariance
def project(self, mean, covariance):
"""Project state distribution to measurement space.
Parameters
----------
mean : ndarray
The state's mean vector (8 dimensional array).
covariance : ndarray
The state's covariance matrix (8x8 dimensional).
Returns
-------
(ndarray, ndarray)
Returns the projected mean and covariance matrix of the given state
estimate.
"""
std = [
self._std_weight_position * mean[3],
self._std_weight_position * mean[3],
1e-1,
self._std_weight_position * mean[3]]
innovation_cov = np.diag(np.square(std))
mean = np.dot(self._update_mat, mean) # 映射矩陣*狀態向量
covariance = np.linalg.multi_dot((
self._update_mat, covariance, self._update_mat.T))
return mean, covariance + innovation_cov # 映射矩陣*狀態向量 + 噪聲矩陣
def update(self, mean, covariance, measurement):
"""Run Kalman filter correction step.
Parameters
----------
mean : ndarray
The predicted state's mean vector (8 dimensional).
covariance : ndarray
The state's covariance matrix (8x8 dimensional).
measurement : ndarray
The 4 dimensional measurement vector (x, y, a, h), where (x, y)
is the center position, a the aspect ratio, and h the height of the
bounding box.
Returns
-------
(ndarray, ndarray)
Returns the measurement-corrected state distribution.
"""
projected_mean, projected_cov = self.project(mean, covariance)
chol_factor, lower = scipy.linalg.cho_factor(
projected_cov, lower=True, check_finite=False) # Cholesky分解
kalman_gain = scipy.linalg.cho_solve(
(chol_factor, lower), np.dot(covariance, self._update_mat.T).T,
check_finite=False).T # 求解卡爾曼增益矩陣
innovation = measurement - projected_mean
new_mean = mean + np.dot(innovation, kalman_gain.T) # 預測值和測量值融合后，新的狀態向量
new_covariance = covariance - np.linalg.multi_dot((
kalman_gain, projected_cov, kalman_gain.T)) # 預測值和測量值融合后，新狀態向量的協方差矩陣
return new_mean, new_covariance
def gating_distance(self, mean, covariance, measurements,
only_position=False):
"""Compute gating distance between state distribution and measurements.
A suitable distance threshold can be obtained from `chi2inv95`. If
`only_position` is False, the chi-square distribution has 4 degrees of
freedom, otherwise 2.
Parameters
----------
mean : ndarray
Mean vector over the state distribution (8 dimensional).
covariance : ndarray
Covariance of the state distribution (8x8 dimensional).
measurements : ndarray
An Nx4 dimensional matrix of N measurements, each in
format (x, y, a, h) where (x, y) is the bounding box center
position, a the aspect ratio, and h the height.
only_position : Optional[bool]
If True, distance computation is done with respect to the bounding
box center position only.
Returns
-------
ndarray
Returns an array of length N, where the i-th element contains the
squared Mahalanobis distance between (mean, covariance) and
`measurements[i]`.
"""
mean, covariance = self.project(mean, covariance)
if only_position:
mean, covariance = mean[:2], covariance[:2, :2]
measurements = measurements[:, :2]
cholesky_factor = np.linalg.cholesky(covariance)
d = measurements - mean
z = scipy.linalg.solve_triangular(
cholesky_factor, d.T, lower=True, check_finite=False,
overwrite_b=True)
squared_maha = np.sum(z * z, axis=0)
return squared_maha
if __name__ == "__main__":
kf = KalmanFilter()
img = cv2.imread("./car.png")
m = np.array([108.5, 360.5, 1.618, 123]) # 小車的初始化測量值，分別表示小車的x_center, y_center, w/h, h
mean_ini, covariance_ini = kf.initiate(m) # 初始化卡爾曼的動態轉移矩陣，映射矩陣
for i in range(30):
mean_pre, covariance_pre = kf.predict(mean_ini, covariance_ini) # 預測狀態變量
dx = np.random.randint(0, 120)
dy = np.random.randint(-30, 30)
m = m + np.array([dx, dy, 0, 0]) # 隨機向左，和上下移動
cv2.rectangle(img, (int(m[0]-m[2]*m[3]/2), int(m[1]-m[3]/2)),
(int(m[0]+m[2]*m[3]/2), int(m[1]+m[3]/2)), (0, 255, 0), 2) # 用綠色框，繪制小車移動后的真實位置
mean_upd, covariance_upd = kf.update(mean_pre, covariance_pre, m+np.random.randn(4)) # 利用測量值，更新狀態變量
mean_ini = mean_upd
covariance_ini = covariance_upd
# 用紅色框，繪制小車移動后的估計位置
cv2.rectangle(img, (int(mean_ini[0] - mean_ini[2] * mean_ini[3] / 2), int(mean_ini[1] - mean_ini[3] / 2)),
(int(mean_ini[0] + mean_ini[2] * mean_ini[3] / 2), int(mean_ini[1] + mean_ini[3] / 2)), (0, 0, 255), 2)
cv2.namedWindow("img", cv2.WINDOW_NORMAL)
cv2.resizeWindow("img", 960, 540)
cv2.imshow("img", img)
cv2.waitKey(0)
cv2.destroyAllWindows()
