描述

*免責聲明*

我不是卡爾曼濾波器專家。只是分享我的故事。也許它對某人有幫助。如果您有任何建議或想法，請隨時在評論中留下它們。

使用卡爾曼濾波器進行定位？

我做了一個在戶外導航的機器人。它使用航路點進行導航。為此，它需要知道它在哪里。這就是大問題開始的地方。知道某事是，并不容易。GPS 看起來很明顯，但并不精確。這是因為您需要移動才能獲得不錯的定位。當附近有樹木和房屋等大物體時，您會得到一種稱為“多路徑”的東西。

使用 IMU？大多數(shù)小型 IMU 不是很一致。您并不總是獲得相同的速度讀數(shù)。在毛茸茸的地形（草）上，情況更糟。

卡爾曼濾波器？卡爾曼濾波器可以做一些叫做“傳感器融合”的事情。它將多個傳感器的輸出組合成一個一致的輸出。

卡爾曼濾波器

卡爾曼濾波器可用作濾波器/平滑器或傳感器融合算法。

卡爾曼濾波器似乎在互聯(lián)網上有很好的記錄。但他們不是。

問題是; 大多數(shù)好的解釋都停留在理論上。他們無法解釋某件事在實踐中的含義。

卡爾曼濾波器的基礎知識

卡爾曼濾波器適用于概率。“系統(tǒng)”/機器人的當前“狀態(tài)”/位置可能位于何處。

機器人當前位置的可能位置。

系統(tǒng)當前狀態(tài)的可能情況。

可能的引擎蓋由“正態(tài)分布”表示。

這個可能的引擎蓋可以與谷歌地圖的位置圈進行比較。“你很可能在這個藍色圓圈內。”

網上有一些很好的解釋。這一個說明了這一點。https://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/

讓我們變得實用

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

有以下矩陣：

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

我使用這個 Python 類進行計算。（作為 np導入的 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)

你想要什么&你有什么？

要開始配置矩陣，您需要問自己兩個問題。

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

X 矩陣

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

Xpos

Ypos

零點

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

速度

速度

速度

X矩陣的最終結果是：

輸入：U 矩陣和 Z 矩陣

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

當您運行卡爾曼濾波器時，您首先要進行預測。做出預測后，您可以做 2 件事：進行另一個預測或更新模型。

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

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

框架

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

• GPS全球框架；經度和緯度
• 局部定位框架；卡爾曼濾波器工作的局部坐標系。它有一個參考點（基站）和一個方向（北）。
• 車身框架；IMU 工作所在的框架。它是機器人相對于本地框架的方向。

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)預測。在 U 矩陣中，最好使用您的高讀取頻率輸入數(shù)據(jù)。就我而言：IMU 數(shù)據(jù)。該矩陣不是必需的。如果您沒有第二個傳感器。

IMU 以 m/s 為單位提供加速度。我們可以使用這個加速度來更新位置和速度。

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

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

斧頭

哎

阿茲

0

0

0

Ax、Ay 和 Az 是轉換到局部坐標系的 XYZ 加速度。3 個零是我們不使用的附加輸入。我稍后會回來。

B矩陣

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

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

加速度與位置之間的關系是加速度的二階導數(shù)。

deltaTime2/2

速度和加速度之間的關系是增量時間。

最終結果將是：

Q矩陣

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

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

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

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

Q 矩陣可以如下所示：

Z矩陣

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

Z 矩陣包含 GPS 測量值。GPS 提供定位經度和緯度。long 和 lat 被轉換為 XYZ 坐標中的局部坐標系（以米為單位）。GPS還提供速度。這些是相對于北的，以公里/小時為單位。我們將速度轉換為 m/s。Z 矩陣將如下所示：

位置X

位置Y

位置Z

速度X

速度Y

速度Z

H矩陣

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

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

R矩陣

R 矩陣包含來自 Z 矩陣的輸入的方差。您僅使用對角線來輸入方差。可以以與 Q 矩陣的方差相同的方式得出方差。

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

F矩陣

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

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

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

P矩陣

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

補充說明

• 卡爾曼濾波器第一次可能不起作用。這是因為方差在現(xiàn)實世界中不是恒定的。傳感器的方差是不斷變化的。GPS模塊在機器人移動時更加準確。當機器人空閑時，加速度為零。這是非常精確的。因為我們知道機器人何時移動。您知道您的電機何時通電。
• 優(yōu)化卡爾曼濾波器非常重要。必須在過濾器運行時調整方差。
• 協(xié)方差矩陣（Q 和 R）之間的比率比數(shù)字更重要。
• 當您知道輸入錯誤時覆蓋輸入是個好主意。如果您知道實數(shù)，覆蓋它可能會有所幫助。

結論

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

在我的項目中。卡爾曼濾波器改進了定位。但我從來沒有讓它按我的意愿工作。獲得低于 1 米的精度很難。