https://skillgreed.com/wp-content/uploads/2020/04/Google-Map-location.jpg

?

網(wǎng)上有一些很好的解釋。這一個(gè)說(shuō)明了這一點(diǎn)。https://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/

讓我們變得實(shí)用

在編程（Python）中是定義在矩陣中的卡爾曼濾波器。這些矩陣包含卡爾曼濾波器的配置。

有以下矩陣：

• X：狀態(tài)（輸出）
• P：狀態(tài)噪聲
• F：狀態(tài)轉(zhuǎn)換模型
• U：預(yù)測(cè)輸入
• B：輸入模型
• Q：過(guò)程噪音
• Z：更新測(cè)量值（輸入）
• H：觀察模型
• R：觀察噪聲

我使用這個(gè) Python 類進(jìn)行計(jì)算。（作為 np導(dǎo)入的 Numpy ）

import numpy as np

class KalmanFilter(object):
    def __init__(self, F = None, B = None, H = None, Q = None
                , R = None, P = None, x0 = None):

        if(F is None or H is None):
            raise ValueError("Set proper system dynamics.")

        self.n = F.shape[1]
        self.m = H.shape[1]

        self.F = F
        self.H = H
        self.B = 0 if B is None else B
        self.Q = np.eye(self.n) if Q is None else Q
        self.R = np.eye(self.n) if R is None else R
        self.P = np.eye(self.n) if P is None else P
        self.x = np.zeros((self.n, 1)) if x0 is None else x0

    def predict(self, u=0):
        self.x = np.dot(self.F, self.x) + np.dot(self.B, u)
        self.P = np.dot(np.dot(self.F, self.P), self.F.T) + self.Q
        return self.x

    def update(self, z):
        y = z - np.dot(self.H, self.x)
        S = self.R + np.dot(self.H, np.dot(self.P, self.H.T))
        K = np.dot(np.dot(self.P, self.H.T), np.linalg.inv(S))
        self.x = self.x + np.dot(K, y)
        I = np.eye(self.n)
        self.P = np.dot(np.dot(I - np.dot(K, self.H), self.P)
                        , (I - np.dot(K, self.H)).T) + np.dot(np.dot(K, self.R)
                        , K.T)

你想要什么&你有什么？

要開(kāi)始配置矩陣，您需要問(wèn)自己兩個(gè)問(wèn)題。

• 你想要什么？卡爾曼濾波器必須給出什么輸出？就我而言，我需要知道機(jī)器人的位置。
• 你有什么？卡爾曼濾波器有哪些輸入數(shù)據(jù)？這可以是 GPS 位置、IMU 加速度……您的輸入數(shù)據(jù)比您想象的要多！大多數(shù) GPS 模塊提供方位和速度。嘗試從傳感器中獲取所有數(shù)據(jù)。

X 矩陣

x 矩陣是卡爾曼濾波器的輸出。如果要預(yù)測(cè)位置，則填寫 XYZ 位置。

Xpos

Ypos

零點(diǎn)

知道速度對(duì)于計(jì)算位置很方便。所以我填寫了XYZ速度。

速度

速度

速度

X矩陣的最終結(jié)果是：

X 矩陣

?

輸入：U 矩陣和 Z 矩陣

有 2 個(gè)輸入。狀態(tài)預(yù)測(cè)的 U 矩陣。狀態(tài)更新的 Z 矩陣。

當(dāng)您運(yùn)行卡爾曼濾波器時(shí)，您首先要進(jìn)行預(yù)測(cè)。做出預(yù)測(cè)后，您可以做 2 件事：進(jìn)行另一個(gè)預(yù)測(cè)或更新模型。

• IMU 的讀取頻率為 100Hz。
• GPS 的讀取頻率為 20Hz。

我使用 IMU 作為狀態(tài)預(yù)測(cè)（U 矩陣）的輸入。用于狀態(tài)更新的較低頻率 GPS。您的卡爾曼濾波器可以做出比更新更多的預(yù)測(cè)。這就是為什么您將高讀取頻率輸入作為狀態(tài)預(yù)測(cè)輸入。

框架

在我們開(kāi)始向卡爾曼濾波器提供數(shù)據(jù)之前。我們必須首先正確格式化數(shù)據(jù)。加速度必須轉(zhuǎn)換為正確的框架和正確的比例。有 3 幀。

• GPS全球框架；經(jīng)度和緯度
• 局部定位框架；卡爾曼濾波器工作的局部坐標(biāo)系。它有一個(gè)參考點(diǎn)（基站）和一個(gè)方向（北）。
• 車身框架；IMU 工作所在的框架。它是機(jī)器人相對(duì)于本地框架的方向。

GPS框架和本地框架

?

import math


def GPS_velocity_to_local(gpsVel, angle):
	vel = gpsVel
	Vx = math.cos(angle) * vel / 3.6 # 3.6 for km/h to m/s
	Vy = math.sin(angle) * vel / 3.6 # 3.6 for km/h to m/s
	v = [Vx, Vy, 0]

	return v

def GPS_position_to_local(gpsRefPosition, gpsCurrentPosition):
	lon1 = gpsRefPosition[0]
	lat1 = gpsRefPosition[1]
	lon2 = gpsCurrentPosition[0]
	lat2 = gpsCurrentPosition[1]

	dx = (lon1 - lon2) * 40000 * math.cos((lat1 + lat2) * math.pi / 360) / 360 * 1000
	dy = (lat1 - lat2) * 40000 / 360 * 1000

	return [dx, dy]

車身框架和局部框架

?

import numpy as np
import math


def IMU_acceleration_to_local(bodyRotation, acceleration):

	eulXYZ = bodyRotation
	y = eulXYZ[0] #yaw
	p = eulXYZ[1] #pitch
	r = eulXYZ[2] #roll

	mes = np.array([
		[acceleration[0]],
		[acceleration[1]],
		[acceleration[2]]
	])
	
	c = math.cos
	s = math.sin
	
	# Direction cosine matrix
	trans = np.array([
		[
			[c(y)*c(p), c(p) * s(y), -s(p)],
			[c(y)*s(p)*s(r)-c(r)*s(y), c(y)*c(r)+s(y)*s(r), c(p)*s(r)],
			[c(y)*c(r)*s(p)+s(y)*s(r), c(r)*s(y)*s(p)-c(y)*s(r), c(p)*c(r)]
		]
	])

	out = np.dot(trans, mes).tolist()[0]
	out = [out[0][0]*-1, out[1][0]*-1, out[2][0]]

	return out

U 矩陣

U 矩陣是卡爾曼濾波器的主要輸入矩陣。它用于狀態(tài)預(yù)測(cè)。在 U 矩陣中，最好使用您的高讀取頻率輸入數(shù)據(jù)。就我而言：IMU 數(shù)據(jù)。該矩陣不是必需的。如果您沒(méi)有第二個(gè)傳感器。

IMU 以 m/s 為單位提供加速度。我們可以使用這個(gè)加速度來(lái)更新位置和速度。

在我們可以使用加速之前。我們需要從身體框架轉(zhuǎn)換到局部框架。我們可以使用 IMU 的絕對(duì)方向。通過(guò)使用方向余弦矩陣，我們可以轉(zhuǎn)換兩幀之間的加速度。

我們的 U 矩陣將如下所示：

斧頭

哎

阿茲

0

0

0

Ax、Ay 和 Az 是轉(zhuǎn)換到局部坐標(biāo)系的 XYZ 加速度。3 個(gè)零是我們不使用的附加輸入。我稍后會(huì)回來(lái)。

U 矩陣

?

B矩陣

B 矩陣包含輸入 (U) 和輸出 (X) 之間的關(guān)系。它告訴卡爾曼濾波器 U 矩陣如何影響狀態(tài)（X 矩陣/輸出）。

?

在左邊，我們有之前的狀態(tài)（X 矩陣）。在中間，我們有 B 矩陣。在頂部，我們有輸入 U 矩陣。在右邊，我們有未來(lái)狀態(tài)（輸出/X 矩陣）。我們希望通過(guò)我們的測(cè)量（U 矩陣/輸入）來(lái)影響未來(lái)的狀態(tài)/輸出。

加速度與位置之間的關(guān)系是加速度的二階導(dǎo)數(shù)。

deltaTime2/2

?

?

?

?

1 / 3

?

速度和加速度之間的關(guān)系是增量時(shí)間。

?

?

?

?

1 / 3

?

最終結(jié)果將是：

B矩陣

?

Q矩陣

Q 矩陣包含來(lái)自 U 矩陣的輸入的方差。它包含您輸入的常見(jiàn)錯(cuò)誤。一個(gè)非常小的數(shù)字意味著您的傳感器/輸入精度很高。高數(shù)字意味著您的輸入非常不準(zhǔn)確。除非未使用，否則不要使用 0。

您在矩陣的對(duì)角線上填寫輸入的方差。

查找輸入的方差。可以從傳感器的數(shù)據(jù)表中獲得傳感器的方差。但是，大多數(shù)時(shí)候這是錯(cuò)誤的。還有另外兩種獲得方差的方法。

• 在系統(tǒng)正常運(yùn)行時(shí)測(cè)量傳感器讀數(shù)。然后同時(shí)用更好的傳感器進(jìn)行測(cè)量以進(jìn)行比較。這種方式不太實(shí)用。除非您用來(lái)比較的測(cè)量值非常精確。
• 在系統(tǒng)空閑/不移動(dòng)時(shí)測(cè)量傳感器讀數(shù)。并將傳感器讀數(shù)與 0 進(jìn)行比較。這會(huì)給您帶來(lái)差異。不是最好的。

Q 矩陣可以如下所示：

Q 矩陣示例

?

Z矩陣

Z矩陣是第二個(gè)輸入矩陣。它用于狀態(tài)更新。通常你把你的低讀取頻率傳感器日期放在這里。

Z 矩陣包含 GPS 測(cè)量值。GPS 提供定位經(jīng)度和緯度。long 和 lat 被轉(zhuǎn)換為 XYZ 坐標(biāo)中的局部坐標(biāo)系（以米為單位）。GPS還提供速度。這些是相對(duì)于北的，以公里/小時(shí)為單位。我們將速度轉(zhuǎn)換為 m/s。Z 矩陣將如下所示：

位置X

位置Y

位置Z

速度X

速度Y

速度Z

Z矩陣

?

H矩陣

H 矩陣類似于 B 矩陣。它包含 Z 矩陣輸入和輸出（X 矩陣）之間的關(guān)系。填充 H 矩陣與填充 B 矩陣的過(guò)程相同。

在這種情況下，GPS 輸入與輸出（X 矩陣）具有 1 對(duì) 1 的關(guān)系。H矩陣是單位矩陣。

H矩陣是middel矩陣

?

H矩陣

?

R矩陣

R 矩陣包含來(lái)自 Z 矩陣的輸入的方差。您僅使用對(duì)角線來(lái)輸入方差?？梢砸耘c Q 矩陣的方差相同的方式得出方差。

我在機(jī)器人靜止時(shí)測(cè)量了 GPS 的偏差。這將為您提供 GPS 的變化。就我而言，R 矩陣如下所示：

R 矩陣示例

?

F矩陣

F 矩陣包含數(shù)據(jù)從狀態(tài)到狀態(tài)的關(guān)系。例如，速度通過(guò)增量時(shí)間影響位置。

我們想要我們未來(lái)狀態(tài)的當(dāng)前位置和速度。我們希望速度通過(guò)增量時(shí)間影響位置。為了將當(dāng)前狀態(tài)傳播到未來(lái)狀態(tài)，我們將從一個(gè)統(tǒng)一矩陣開(kāi)始。

F 矩陣示例（state-1 = state）

?

現(xiàn)在我們希望速度通過(guò)增量時(shí)間影響位置。

F 矩陣示例（速度影響位置）

?

P矩陣

P 矩陣包含當(dāng)前狀態(tài)的方差（X 矩陣）。卡爾曼濾波器將自行更新值。您可以將其初始化為單位矩陣。或者，您可以使用之前運(yùn)行的 P 矩陣。當(dāng)您使用上一次運(yùn)行的 P 矩陣時(shí)。卡爾曼濾波器在開(kāi)始時(shí)會(huì)更準(zhǔn)確。

P 矩陣示例

?

補(bǔ)充說(shuō)明

• 卡爾曼濾波器第一次可能不起作用。這是因?yàn)榉讲钤诂F(xiàn)實(shí)世界中不是恒定的。傳感器的方差是不斷變化的。GPS模塊在機(jī)器人移動(dòng)時(shí)更加準(zhǔn)確。當(dāng)機(jī)器人空閑時(shí)，加速度為零。這是非常精確的。因?yàn)槲覀冎罊C(jī)器人何時(shí)移動(dòng)。您知道您的電機(jī)何時(shí)通電。
• 優(yōu)化卡爾曼濾波器非常重要。必須在過(guò)濾器運(yùn)行時(shí)調(diào)整方差。
• 協(xié)方差矩陣（Q 和 R）之間的比率比數(shù)字更重要。
• 當(dāng)您知道輸入錯(cuò)誤時(shí)覆蓋輸入是個(gè)好主意。如果您知道實(shí)數(shù)，覆蓋它可能會(huì)有所幫助。

結(jié)論

卡爾曼濾波器是過(guò)濾噪聲和組合傳感器的好算法。但是，它有其局限性。這不是魔術(shù)。如果您的傳感器數(shù)據(jù)是垃圾，卡爾曼濾波器的結(jié)果也不會(huì)好很多。IMU的不一致是個(gè)大問(wèn)題。GPS模塊的多路徑是一個(gè)問(wèn)題。有些是可以解決的。您可以使用更多高質(zhì)量的傳感器。但它變得非常迅速。

在我的項(xiàng)目中。卡爾曼濾波器改進(jìn)了定位。但我從來(lái)沒(méi)有讓它按我的意愿工作。獲得低于 1 米的精度很難。