卡爾曼濾波器的特性及仿真

我們前一篇關(guān)于人物識別跟蹤的文章《視頻連續(xù)目標(biāo)跟蹤實(shí)現(xiàn)的兩種方法和示例(更新)》里講到，視頻圖像中物體的識別和跟蹤用到了卡爾曼濾波器(KF)。這里對這個(gè)話題我們稍微對這個(gè)卡爾曼濾波器進(jìn)行一個(gè)整理。

目標(biāo)跟蹤為什么用到卡爾曼濾波器(KF)?其實(shí)是因?yàn)槲覀兗僭O(shè)了視頻的連續(xù)幀中的每個(gè)物體在圖像中的移動(dòng)位置不會相差太多來進(jìn)行的。我們在卡爾曼濾波器中設(shè)計(jì)了一個(gè)運(yùn)動(dòng)模式，這個(gè)運(yùn)動(dòng)模式可以讓我們估計(jì)在下一個(gè)圖像中每個(gè)被識別物體的所在位置，把每個(gè)位置和上一圖像中的位置進(jìn)行比較統(tǒng)計(jì)，就可以定個(gè)七七八八了。

KF本質(zhì)上是一種用于估計(jì)動(dòng)態(tài)系統(tǒng)狀態(tài)的遞歸算法。它通過對系統(tǒng)的數(shù)學(xué)建模，輔以噪聲和不確定性假設(shè)，能夠從一系列不精確和噪聲污染的測量數(shù)據(jù)中推斷出系統(tǒng)的最優(yōu)狀態(tài)估計(jì)。其核心思想是利用先驗(yàn)知識和新獲得的測量信息來更新對系統(tǒng)狀態(tài)的估計(jì)，使得估計(jì)誤差最小化。誤差、噪聲干擾，這些設(shè)定符合我們實(shí)際應(yīng)用場景中的所能得到的條件。而只有當(dāng)信噪比足夠高，或者誤差幾乎可以忽略不計(jì)，并且構(gòu)建的系統(tǒng)數(shù)學(xué)模型足夠準(zhǔn)確時(shí)，才可以對測量計(jì)算得到的結(jié)果抱有信心，否則我們就需要在不完美的現(xiàn)實(shí)數(shù)據(jù)中，進(jìn)行綜合

KF廣泛用于各種應(yīng)用場景，特別是在需要高精度和實(shí)時(shí)處理的領(lǐng)域，如導(dǎo)航系統(tǒng)、自動(dòng)駕駛技術(shù)、信號處理和控制系統(tǒng)等。

卡爾曼濾波器有多種類型，小編這里將只圍繞標(biāo)準(zhǔn)卡爾曼濾波器(KF)和擴(kuò)展卡爾曼濾波器(Extended KF - EKF)講一點(diǎn)特性。

經(jīng)典卡爾曼濾波器(KF)：

特點(diǎn)：適用于線性系統(tǒng)假設(shè)的隨機(jī)信號處理。包含兩步：預(yù)測和更新，每步都使用線性方程。

應(yīng)用：常用于導(dǎo)航、控制系統(tǒng)及金融工程中

擴(kuò)展卡爾曼濾波器(EKF)：

特點(diǎn)：擴(kuò)展了標(biāo)準(zhǔn)KF，將其概念應(yīng)用到非線性系統(tǒng)中。通過對非線性方程進(jìn)行線性化處理，使用雅可比矩陣來估計(jì)狀態(tài)。簡單地講，就是把分線性方程在每個(gè)取值點(diǎn)處進(jìn)行泰勒展開

應(yīng)用：常用于非線性系統(tǒng)中，如機(jī)器人定位、航空航天導(dǎo)航。

卡爾曼濾波器是一種用于估計(jì)動(dòng)態(tài)系統(tǒng)狀態(tài)的最優(yōu)遞歸算法，其核心基于以下兩組基本狀態(tài)方程模型：

	線性離散狀態(tài)空間方程	非線性離散狀態(tài)空間方程
過程方程

觀測方程

KF對應(yīng)的是上面的線性離散狀態(tài)方程，EKF對應(yīng)的是上面的非線性離散狀態(tài)空間方程。

上面提到的非線性離散狀態(tài)空間方程中，噪聲特性是非加性噪聲。如果是加性噪聲，那么對應(yīng)下面的公式：

????????（1）????????

????????（2）????????

但我們完全可以把這種加性噪聲作為非加性噪聲的一個(gè)特例處理。后面會提到并說明。

請記住這里的表示的系統(tǒng)狀態(tài)一般都是一個(gè)多維向量，所以我們這里討論隨機(jī)狀態(tài)分布的噪聲w和v兩個(gè)隨機(jī)噪聲變量其實(shí)是期望值等于0，協(xié)方差矩陣分別為Q、R的正態(tài)分布的矩陣。其中：

：nx1狀態(tài)向量，包含系統(tǒng)在時(shí)間步 ( k ) 的狀態(tài)；

A是狀態(tài)轉(zhuǎn)移矩陣，定義了系統(tǒng)的狀態(tài)如何從一個(gè)時(shí)間步轉(zhuǎn)移到下一個(gè)時(shí)間步；

u是px1控制輸入向量，描述了在時(shí)間步 ( k ) 作用于系統(tǒng)的外部控制輸入；

B是控制輸入矩陣，描述控制輸入如何影響狀態(tài)轉(zhuǎn)移；

是觀測向量，包含系統(tǒng)在時(shí)間步(k)的測量值或觀測值；

H是觀測矩陣，將系統(tǒng)狀態(tài)映射到觀測空間；

w通常假設(shè)為均值為零的高斯噪聲，影響狀態(tài)轉(zhuǎn)移；

v通常假設(shè)為均值為零的高斯噪聲，影響觀測；

f(x,u)，狀態(tài)轉(zhuǎn)移函數(shù)，描述系統(tǒng)狀態(tài)如何通過非線性函數(shù)從一個(gè)時(shí)間步轉(zhuǎn)移到下一個(gè)時(shí)間步；

h(x)，控制輸入函數(shù)，用于描述控制輸入如何與系統(tǒng)狀態(tài)通過非線性函數(shù)關(guān)聯(lián)。

對于正態(tài)分布的信號，經(jīng)過線性變換之后對應(yīng)的信號仍然符合正態(tài)分布；而經(jīng)過非線性變換之后的非加性噪聲，一般就失去了正態(tài)分布的特性。示例如下：

一個(gè)正態(tài)分布的信號x~N(0, 1)，分別經(jīng)過:

經(jīng) Y = 3x + 3 轉(zhuǎn)換或者映射之后，在0點(diǎn)的x取值是正態(tài)分布的情況下，線性變換后的Y仍然是符合正態(tài)分布，不過期望值=3，而Y的信號的方差變成了9。

E[Y] = E[3x+3] =E[3x] + 3 =3E[x] + 3 = 3*0 + 3 = 3

D[Y]=E{[Y - E(Y)]^2} =E{[3x+3 - 3]^2} =E[9x^2] =9E[x^2]=9

經(jīng)Z = sin(x) 非線性轉(zhuǎn)換后，雖然其期望值（均值）仍然為0，但是分布不再符合正態(tài)分布的特性。

[圖-1]正態(tài)分布經(jīng)過不同變換前后得到的分布比較

大家可以用下面的代碼用不同的轉(zhuǎn)換進(jìn)行正態(tài)分布經(jīng)歷不同的轉(zhuǎn)換后的分布模擬。

import numpy as np
import matplotlib.pyplot as plt


# Parameters for the normal distribution
mu_x = 0      # Mean of X
sigma_x = 1   # Standard deviation of X


# Linear transformation parameters
a = 3         # Scale factor
b = 3         # Shift factor


# Generate random samples from the normal distribution (X)
x = np.random.normal(mu_x, sigma_x, 5000)


# Apply the linear and nonlinear transformations
y = a * x + b
z = np.sin(x)


# 假設(shè) x, y, z 是你的數(shù)據(jù)，mu_x, sigma_x, a, b 是參數(shù)。


# 設(shè)置子圖
fig, axes = plt.subplots(3, 1, figsize=(15, 5))  # 1 行，3 列


# 在第一個(gè)子圖中繪制 X 的直方圖
axes[0].hist(x, bins=1000, density=True, alpha=0.5, color='blue', label=f'X ~ N({mu_x}, {sigma_x}2)')
axes[0].set_xlabel('X')
axes[0].set_ylabel('Density')
axes[0].set_xlabel('Value')
axes[0].set_ylabel('Probability Density')
axes[0].set_title('Normal Distribution')
axes[0].legend()
# 在第二個(gè)子圖中繪制 Y 的直方圖
axes[1].hist(y, bins=1000, density=True, alpha=0.5, color='orange', label=f'Y = {a}X + ')
axes[1].set_xlabel('Y')
axes[1].set_ylabel('Density')
axes[1].set_xlabel('Value')
axes[1].set_ylabel('Probability Density')
axes[1].set_title('Distribution Of Linear Transformations')
axes[1].legend()
# 在第三個(gè)子圖中繪制 Z 的直方圖
axes[2].hist(z, bins=1000, density=True, alpha=0.5, color='green', label=r'Z = sin(X)')
axes[2].set_xlabel('Z')
axes[2].set_ylabel('Density')
axes[2].set_xlabel('Value')
axes[2].set_ylabel('Probability Density')
axes[2].set_title('Distribution Of Nonlinear Transformations')
axes[2].legend()
# 添加整體標(biāo)題
plt.suptitle('Distributions of X, Y and Z')


# 顯示圖表
plt.tight_layout()
plt.show()

我們再回到EKF中，對于非線性系統(tǒng)的狀態(tài)方程，是通過對非線性系統(tǒng)的泰勒一次展開的方式在小區(qū)間內(nèi)線性化推導(dǎo)（如我們在《紅外熱電堆的特性淺析及應(yīng)用》中的對熱電堆內(nèi)部結(jié)構(gòu)熱傳遞的處理，以及《NTC測溫—查表計(jì)算vs公式計(jì)算》中局部的線性化處理等），然后得到的處理公式；而對于加性噪聲和非加性噪聲，在最后的遞歸處理步驟中，我們會看到不同的表達(dá)處理方式。
小編這里不對KF或者EKF最后的解算步驟進(jìn)行推演，經(jīng)常從卡爾曼濾波器中的兩個(gè)方程結(jié)果的示意圖中看到，KF解決的問題就是如何從兩種手段得到的系統(tǒng)狀態(tài)都是隨機(jī)數(shù)的情況下，如何來獲取更優(yōu)結(jié)果。在收斂的情況下，我們通過KF或者EKF往往可以得到更優(yōu)值。

[圖-2]卡爾曼濾波器的狀態(tài)、測量和輸出噪聲分布

由于過程噪聲w~N(0,Q)和測量噪聲v~N(0,R)的存在，讓我們在根據(jù)模型得到的被噪聲干擾后的狀態(tài)隨機(jī)值和由傳感器或者其他數(shù)據(jù)采集設(shè)備得到的另外一組狀態(tài)隨機(jī)值之間需要做一個(gè)平衡后的輸出，最后的狀態(tài)值，也肯定會偏向在Q或者R中那個(gè)協(xié)方差更小的值。
有很多的書，網(wǎng)上也有很多的文章或者視頻介紹KF和EKF的推演的過程。有關(guān)文獻(xiàn)見本文參考部分。
必須要吐槽一下公眾號無法使用“圈”外的weblink，否則就直接上網(wǎng)頁鏈接，而不是拷貝鏈接了。

[圖-3]KF計(jì)算步驟[1][3]
在KF中，狀態(tài)轉(zhuǎn)移矩陣A，控制輸入向量u，控制輸入矩陣B，觀測矩陣H，測量向量Zk，過程噪聲協(xié)方差Q，測量噪聲協(xié)方差R，以及必要的后驗(yàn)估計(jì)狀態(tài)值的初始值，后驗(yàn)估計(jì)誤差協(xié)方差等都是已知的參數(shù)，就可以進(jìn)行計(jì)算了：

從先驗(yàn)估計(jì)值開始第一步計(jì)算；

然后計(jì)算后驗(yàn)估計(jì)誤差協(xié)方差；

計(jì)算Kalman增益；

代入測量向量Zk和卡爾曼增益，得到后驗(yàn)估計(jì)值(狀態(tài)的中間結(jié)果)；

迭代得到后驗(yàn)估計(jì)誤差協(xié)方差，為下一次迭代準(zhǔn)備。 ?

[圖-4]EKF的計(jì)算步驟(非加性噪聲)[1][2][3]

在上面的EKF計(jì)算步驟中，符號名稱很多還是一致的，但實(shí)際操作上存在一些差異。下面是它的操作步驟。

從先驗(yàn)估計(jì)值開始第一步計(jì)算；

計(jì)算雅可比矩陣：

然后計(jì)算后驗(yàn)估計(jì)誤差協(xié)方差；

計(jì)算Kalman增益，計(jì)算該增益前，包括以下兩個(gè)雅可比計(jì)算子項(xiàng)：

代入測量向量Zk和卡爾曼增益，得到后驗(yàn)估計(jì)值(狀態(tài)的中間結(jié)果)；

迭代得到后驗(yàn)估計(jì)誤差協(xié)方差，為下一次迭代準(zhǔn)備。

如果是加性噪聲呢？我們再看前面的公式(1)和(2)，這時(shí)用下面的fA和hA來替代對應(yīng)的公式：

這種情況下，對和分別對w和v求偏導(dǎo)時(shí)候，得到的雅可比矩陣都是單位對角矩陣I，Wk和Vk就可以直接消掉了。 在EKF中，雅可比矩陣用于線性化非線性系統(tǒng)模型，并將其應(yīng)用于狀態(tài)更新和測量更新的過程。例如下面狀態(tài)方程對狀態(tài)變量的偏導(dǎo)數(shù)。

對于狀態(tài)更新，雅可比矩陣是系統(tǒng)狀態(tài)方程對狀態(tài)變量的偏導(dǎo)數(shù)，通常這個(gè)雅可比矩陣的維度是n×n，其中n是狀態(tài)變量的數(shù)量。

對于測量更新，雅可比矩陣是觀測方程對狀態(tài)變量的偏導(dǎo)數(shù)，其維度通常是m×n，其中m是測量變量的數(shù)量。如果m和n相等，那么這個(gè)情況下的雅可比矩陣也是方形的。

KF示例

在該示例中，我們用平拋物體的軌跡使用KF進(jìn)行處理。按道理，因?yàn)榇嬖?/2*g*t^2，這應(yīng)該是一個(gè)非線性系統(tǒng)，能夠用狀態(tài)轉(zhuǎn)移矩陣(A)建立一個(gè)線性狀態(tài)模型，如x下面代碼所示，那么卡爾曼濾波器（KF）就足夠了。

卡爾曼濾波器適用于線性高斯?fàn)顟B(tài)模型和觀測模型的情況。在示例的模型中，狀態(tài)轉(zhuǎn)移矩陣(A)是線性的，因?yàn)樗哂泄潭ǖ南禂?shù)來描述位置、速度和加速度之間的關(guān)系，而沒有非線性項(xiàng)。如果你的測量模型（測量方程的矩陣）也是線性的，那么你可以直接使用標(biāo)準(zhǔn)的卡爾曼濾波器解決問題。

示例中，我們給定的初始條件：

x為水平方向，初始位置=0；

vx為水平速度方向，初始速度=3m/s:

x = v0 + vx * t；

y為垂直方向，初始位置和速度都是為0

y = y0 + vy0 * t +1/2 * g * t^2；

垂直方向只有加速度g，水平方向沒有阻力；

過程噪聲協(xié)方差Q賦值較?。?/p>

Q = np.eye(5) * 0.02 # Process noise covariance

Q[4,4] = 0 # 1g - gravity acceleration，穩(wěn)定無噪聲

測量噪聲協(xié)方差R賦值稍大：

R = np.eye(2) * 1.5 # Measurement noise covariance

初始的后驗(yàn)估計(jì)狀態(tài)誤差協(xié)方差P較大(我們要看一下收斂特性)：

P = np.eye(5) * 50

先看結(jié)果后看模擬代碼。為了彰顯KF的功能，把觀測狀態(tài)的協(xié)方差設(shè)置的稍微大了些，而把狀態(tài)轉(zhuǎn)移方程的誤差協(xié)方差設(shè)置得更小一些?？梢钥吹綆缀醢l(fā)散的狀態(tài)的測量觀測值（紅線），KF的后驗(yàn)狀態(tài)估計(jì)值都更靠近狀態(tài)轉(zhuǎn)移方程得到的數(shù)值。

[圖-5]KF處理拋物線仿真（實(shí)藍(lán)線）

[圖-6]KF處理拋物線仿真x方向的位置后驗(yàn)估計(jì)誤差協(xié)方差的收斂

特點(diǎn)說明：

盡管觀測測量的狀態(tài)亂七八糟，但是幸虧狀態(tài)轉(zhuǎn)移的模型足夠準(zhǔn)確，讓最終的后驗(yàn)狀態(tài)估計(jì)，那條實(shí)藍(lán)線，仍然可以繞在理論軌跡附近；

盡管后驗(yàn)估計(jì)狀態(tài)誤差協(xié)方差P較大（50），但是KF處理過程中，我們可以看到該協(xié)方差以非?？斓乃俣染褪諗康搅艘粋€(gè)較小且穩(wěn)定的數(shù)值。代碼中對標(biāo)準(zhǔn)差范圍取值有補(bǔ)充說明。圖-6中的灰色輪廓線只用到了+/-2σ的公差范圍。

拋物線KF模擬代碼：

import numpy as np
import matplotlib.pyplot as plt


# Define constants
dt = 0.05  # time interval
g = 9.81  # gravitational acceleration


# Initial state [x, vx, y, vy, ay]
initial_state = np.array([0, 3, 0, 0, -g])  # initial position (0,0) with vx0=3, vy0=0


# State transition matrix
F = np.array([[1, dt, 0, 0, 0],
              [0, 1, 0, 0, 0],
              [0, 0, 1, dt, 0.5 * dt**2],
              [0, 0, 0, 1, dt],
              [0, 0, 0, 0, 1]])


# Observation matrix
H = np.array([[1, 0, 0, 0, 0], 
              [0, 0, 1, 0, 0]])


# Process and measurement noise matrices
Q = np.eye(5) * 0.02    # Process noise covariance
Q[4,4] = 0              # 1g - gravity acceleration


R = np.eye(2) * 1.5     # Measurement noise covariance


# Initial estimation covariance matrix
P = np.eye(5) * 50


# Lists to store positions
posterior_state_estimate, ideal_positions, measurements = [], [], []
prior_state_estimate = []
# Time steps for simulation
n_steps = 51


# Initialize state variables
state = initial_state.copy()
ideal_state = initial_state.copy()


# 生成過程噪聲，可以使用 Q 的對角線元素生成
process_noise_variance = np.diag(Q)  # 假設(shè)是一個(gè)對角矩陣


# 用于存儲方差的列表
position_X_variances = []
position_X_variances.append(P[0, 0])
position_Y_variances = []
position_Y_variances.append(P[2, 2])


process_state = initial_state.copy()
z_state_measure = initial_state[[0, 2]]


for _ in range(n_steps):
    # 狀態(tài)空間方程得到的模擬數(shù)據(jù)
    process_noise = np.random.normal(0, process_noise_variance, size=state.shape)
    process_state = F @ process_state + process_noise


    # 模擬測量得到的狀態(tài)：measurement state
    z_state_measure = H @ process_state + np.random.normal(0, np.sqrt(R.diagonal()))


    # Prediction step
    process_noise = np.random.normal(0, process_noise_variance, size=state.shape)
    state = F @ state + process_noise
    P = F @ P @ F.T + Q


    prior_state_estimate.append((state[0], state[2]))
    # Assume we get some measurements (with noise)
    ideal_state = F @ ideal_state
    #z_state_measure = H @ ideal_state + np.random.normal(0, np.sqrt(R.diagonal()))


    # Kalman Gain
    S = H @ P @ H.T + R
    K = P @ H.T @ np.linalg.inv(S)


    # Update step
    y = z_state_measure - H @ state
    state += K @ y
    P = (np.eye(len(K)) - K @ H) @ P


    # 存儲 x，y 位置的方差
    position_X_variances.append(P[0, 0])
    position_Y_variances.append(P[2, 2])
    # Store the results
    posterior_state_estimate.append((state[0], state[2]))
    ideal_positions.append((ideal_state[0], ideal_state[2]))
    measurements.append((z_state_measure[0], z_state_measure[1]))


# Convert lists to numpy arrays
prior_state_estimate = np.array(prior_state_estimate)
posterior_state_estimate = np.array(posterior_state_estimate)
ideal_positions = np.array(ideal_positions)
measurements = np.array(measurements)


# Plot the results
plt.figure(figsize=(10, 6))
plt.plot(prior_state_estimate[:,0],prior_state_estimate[:,1], label = 'Prior State Estimate',linestyle='--')
plt.plot(ideal_positions[:, 0], ideal_positions[:, 1], label='Ideal Trajectory', linestyle='--')
#plt.scatter(measurements[:, 0], measurements[:, 1], c='orange', label='Measurements', marker='o')
plt.plot(measurements[:, 0], measurements[:, 1], c='red', label='Measurements', linestyle='-')
plt.plot(posterior_state_estimate[:, 0], posterior_state_estimate[:, 1], c='blue', label='Posterior Estimated Trajectory', linestyle='-')
plt.xlabel('x position (m)')
plt.ylabel('y position (m)')
plt.title('Projectile Motion with Kalman Filter - Ideal vs Estimated vs Measured')
plt.legend()
plt.grid()
plt.show()


# 繪制X位置的方差隨時(shí)間的變化
"""
1倍標(biāo)準(zhǔn)差（±1σ）：大約68.27%的數(shù)據(jù)落在一個(gè)標(biāo)準(zhǔn)差范圍內(nèi)，即從分布平均值向左右各一個(gè)標(biāo)準(zhǔn)差。
2倍標(biāo)準(zhǔn)差（±2σ）：大約95.45%的數(shù)據(jù)落在兩個(gè)標(biāo)準(zhǔn)差范圍內(nèi)，這一個(gè)常用的置信區(qū)域，許多統(tǒng)計(jì)分析中常被用于定義數(shù)據(jù)的正常范圍。
3倍標(biāo)準(zhǔn)差（±3σ）：大約99.73%的數(shù)據(jù)落在三個(gè)標(biāo)準(zhǔn)差范圍內(nèi)，通常用于識別異常值或者極端值。
"""
time_steps = range(n_steps+1)
plt.figure(figsize=(10, 5))
plt.plot(time_steps, position_X_variances, label='Variance of X Position', color='blue')
# Calculate twice the standard deviation
twice_std_dev = 2 * np.sqrt(position_X_variances)
plt.fill_between(time_steps, 
                 0 - np.sqrt(twice_std_dev), 
                 0 + np.sqrt(twice_std_dev), 
                 color='gray', alpha=0.5, step='mid', label='2 Std Dev Range')
plt.xlabel('Time Step')
plt.ylabel('Variance')
plt.title('Convergence of X Position Variance')
plt.legend()
plt.grid(True)
plt.show()


# 繪制Y位置的方差隨時(shí)間的變化
plt.figure(figsize=(10, 5))
plt.plot(time_steps, position_Y_variances, label='Variance of Y Position', color='blue')
# Calculate twice the standard deviation
twice_std_dev = 2 * np.sqrt(position_Y_variances)
plt.fill_between(time_steps, 
                 0 - np.sqrt(twice_std_dev), 
                 0 + np.sqrt(twice_std_dev), 
                 color='gray', alpha=0.5, step='mid', label='2 Std Dev Range')
plt.xlabel('Time Step')
plt.ylabel('Variance')
plt.title('Convergence of Y Position Variance')
plt.legend()
plt.grid(True)
plt.show()

EKF示例

該示例中，模擬一輛小車始終以沿著其自身的中軸線用速度V行駛，而且小車整體還以固定的轉(zhuǎn)速轉(zhuǎn)動(dòng)。我們以這個(gè)為前提對其行駛軌跡[x,y]和轉(zhuǎn)動(dòng)角度進(jìn)行跟蹤輸出。

[圖-7]模擬小車的行駛和轉(zhuǎn)動(dòng)

我們可以根據(jù)條件得到以下的狀態(tài)轉(zhuǎn)移方程(3)和觀測測量方程(4)：

(3)

(4)

其中，w~N(0, Q)和v~[0, R]分別是過程噪聲和測量誤差噪聲。

根據(jù)離散狀態(tài)方程，我們可以得到[圖-4]中的和：

（5）

（6）

而[圖-4]中的Wk和Vk則是兩個(gè)單位矩陣。上面公式(5)和(6)需要留意代入的值。

先上圖后看代碼。仿真該小車逐漸轉(zhuǎn)彎的行駛軌跡和轉(zhuǎn)動(dòng)角度的大小如下圖所示。

[圖-8]EKF模擬輸出的軌跡和轉(zhuǎn)動(dòng)角度

如論如何，如果狀態(tài)方程和觀測方程的數(shù)值都不可靠，那么無論KF還是EKF，可能都將無法得到我們期望的結(jié)果。
EKF示例小車軌跡和轉(zhuǎn)角模擬代碼如下：

import numpy as np
import matplotlib.pyplot as plt


# A state space model for a differential drive mobile robot


# A matrix - 3x3 identity matrix
A_t_minus_1 = np.array([[1.0,  0,  0],
                        [  0,1.0,  0],
                        [  0,  0, 1.0]])


# Function to get the B matrix
def getB(yaw, dt):
    """
    Calculates and returns the B matrix
    3x2 matrix -> number of states x number of control inputs
    Expresses how the state of the system [x,y,yaw] changes
    due to the control commands
    :param yaw: The yaw angle in radians
    :param dt: The time change in seconds
    """
    B = np.array([[np.cos(yaw) * dt, 0],
                  [np.sin(yaw) * dt, 0],
                  [0, dt]])
    return B


# Define Jacobian for the transition function
def jacobian_of_motion(state, control, dt):
    _, _, yaw = state
    v, _ = control
    J_f = np.array([
        [1, 0, -v * np.sin(yaw) * dt],
        [0, 1, v * np.cos(yaw) * dt],
        [0, 0, 1]
    ])
    return J_f


def ekf_simulation(initial_state, control_vector, total_time, time_step):
    # Initialize state for EKF
    state_estimate = initial_state
    states_over_time = [state_estimate]


    # Simulated and ideal states tracking
    simulated_states = [initial_state]
    ideal_states = [initial_state]
    prior_states_estimate = [initial_state]
    
    # Initial posterior covariance of new estimate
    Pk = np.diag([1.0, 1.0, 1.0])


    # Measurement noise covariance matrix R_k
    # Measurement Noise init
    std_dev_x_mes = 0.5    # Standard deviation for x measurement
    std_dev_y_mes = 0.5    # Standard deviation for y measurement
    std_dev_yaw_mes = 0.15  # Standard deviation for yaw measurement
    
    R_k = np.diag([std_dev_x_mes**2, std_dev_y_mes**2, std_dev_yaw_mes**2])
    
    H = np.array([[1, 0, 0],
                  [0, 1, 0],
                  [0, 0, 1]])


    # Jacobian matrix J_h, which is the same as H for a linear model
    J_h = H
    
    # Process Noise init
    std_dev_x = 0.15
    std_dev_y = 0.15
    std_dev_yaw = 0.15


    # Simulation loop
    for _ in np.arange(0, total_time, time_step):
        # Ideal state (no noise)
        ideal_state = A_t_minus_1 @ ideal_states[-1] + getB(ideal_states[-1][2], time_step) @ control_vector
        ideal_states.append(ideal_state)


        # 1-1. State Prediction step
        # 生成過程噪聲
        process_noise_sim = np.random.normal(0, [std_dev_x, std_dev_y, std_dev_yaw], size=ideal_state.shape)


        Prior_state_Estimate = (A_t_minus_1 @ states_over_time[-1] + getB(states_over_time[-1][2], time_step) @ control_vector
                                + process_noise_sim)
        prior_states_estimate.append(Prior_state_Estimate)
        # 1-2. Process Error covariance
        Jf = jacobian_of_motion(states_over_time[-1], control_vector, dt=time_step)
        
        # Generate process noise for simulation as additive noise
        process_noise = np.random.normal(0, [std_dev_x, std_dev_y, std_dev_yaw])
        # Convert to a diagonal covariance matrix if needed for use in the EKF
        process_noise_cov = np.diag(process_noise**2)   
        
        Pk = Jf @ Pk @ Jf.T +  process_noise_cov #np.random.uniform(-process_noise, process_noise, 3)     
        
        # 2-1. Compute the Kalman gain
        K_k = Pk @  J_h.T @ np.linalg.inv(J_h @ Pk @ J_h.T + R_k)


        # Simulate sensor reading with measurement noise (using ideal_state)
        #tolerance_noise = np.random.uniform(-measurement_noise, measurement_noise, 3)
        measurement_noise = np.random.normal(0, [std_dev_x_mes, std_dev_y_mes, std_dev_yaw_mes])
        simulated_state = ideal_state + measurement_noise
        
        simulated_states.append(simulated_state)
        
        # 2-2. Update estimate with measurement z_k
        state_estimate = Prior_state_Estimate + K_k @ (simulated_state - J_h @ Prior_state_Estimate)
        
        # 2-3. Update the error covariance
        Pk = (np.eye(3) - K_k @ J_h) @ Pk


        # Record the updated state
        states_over_time.append(state_estimate)


    return np.array(prior_states_estimate), np.array(states_over_time), np.array(simulated_states), np.array(ideal_states)


def plot_states(prior_states_estimate, states_over_time, simulated_states, ideal_states):
    plt.figure(figsize=(12, 8))


    # Plot position on XY plane
    plt.subplot(2, 1, 1)
    plt.plot(ideal_states[:, 0], ideal_states[:, 1], 'g-', label='Ideal Path')
    plt.plot(prior_states_estimate[:,0],prior_states_estimate[:,1], label='prior Path estimate')
    plt.plot(simulated_states[:, 0], simulated_states[:, 1], 'b--', label='Simulated Measure Path')
    plt.plot(states_over_time[:, 0], states_over_time[:, 1], 'r-', label='EKF posterior state Path')


    plt.xlabel('X Position (meters)')
    plt.ylabel('Y Position (meters)')
    plt.title('Trajectory Over Time')
    plt.legend()
    plt.grid(True)


    # Plot yaw angle over time
    plt.subplot(2, 1, 2)
    time_points = np.arange(0, len(states_over_time))
    plt.plot(time_points, ideal_states[:, 2], 'g-', label='Ideal Yaw')
    plt.plot(prior_states_estimate[:,2], label='prior Yaw estimate')
    plt.plot(time_points, simulated_states[:, 2], 'b--', label='Simulated Measure Yaw')
    plt.plot(time_points, states_over_time[:, 2], 'r-', label='EKF posterior state Yaw')


    plt.xlabel('Time Step')
    plt.ylabel('Yaw (radians)')
    plt.title('Yaw Angle Over Time')
    plt.legend()
    plt.grid(True)


    plt.tight_layout()
    plt.show()


def main():
    # Initial conditions
    initial_state = np.array([0.0, 0.0, 0.0]) # [x_init, y_init, yaw_init]
    control_vector = np.array([4.5, 0.15])    # [v, yaw_rate]


    # Driving parameters
    total_time = 10.0 # Total simulation time in seconds
    time_step = 0.05   # Time step for each iteration in seconds


    # Run simulation
    prior_states_estimate, ekf_states, simulated_states, ideal_states = ekf_simulation(
        initial_state, control_vector, total_time=total_time, time_step=time_step)


    # Plot the simulation results
    plot_states(prior_states_estimate, ekf_states, simulated_states, ideal_states)


main()

大家可以對照模擬中的代碼操作不走和[圖-4]及和EKF操作步驟的描述部分，看看模擬是如何進(jìn)行的，是否還有沒有考慮的問題。以上仿真的輸出圖中，我們都加入了理想狀態(tài)值作為參考和比較。