文章目录
- 简介
- UKF滤波
- 1. 概述和流程
- 2. Python代码
- 第一个版本
- a. KF滤波
- b. UKF滤波
- 第一个版本
简介
上一篇文章，我们介绍了UKF滤波公式及其MATLAB代码。在做视觉测量的过程中，基于OpenCV的开发包比较多，因此我们将UKF的MATLAB代码转到python中，实现数据滤波效果。
UKF滤波
1. 概述和流程
UKF的公式这里就不再过多介绍了，具体内容请参见博客：UKF滤波公式及其MATLAB代码
这里简单把上一篇文章的公式和流程图粘贴一下。
求解流程 ：
-
相比于一般的卡尔曼滤波，UKF算法增加了两次无迹变换，公式为：
权重和方差计算公式为：
-
Sigma点传播：
- 计算x的预测值和协方差矩阵：
4. 得到一组新的Sigma点：
5. 代入观测方程中，得到测量量的预估值：
- 获得观测量的预测值和协方差矩阵：
- 更新状态变量和协方差矩阵：
另外，每次写论文画卡尔曼流程图中，都找不到参考的模板。我自己画了个滤波流程图，不一定符合每个人的审美，以备参考：
2. Python代码
重点来了。。。
上代码。
第一个版本
UKF的python代码我一共写了两个版本。
第一个是我用ChatGPT直接生成了一个，经过数据实测，结果有点奇怪，不太像我想要的结果，每个模块之间的交互也跟我理解的不太一样。为了对比，这里也还是贴出来了，人家写的确实比我写的看着逼格好点。
ChatGPT输出的Python：
a. KF滤波
class KalmanFilter:
def __init__(self, F, H, Q, R, P, x0):
self.F = F
self.H = H
self.Q = Q
self.R = R
self.P = P
self.x = x0
def predict(self):
self.x = self.F @ self.x
self.P = self.F @ self.P @ self.F.T + self.Q
def update(self, z):
y = z - self.H @ self.x
S = self.H @ self.P @ self.H.T + self.R
K = self.P @ self.H.T @ np.linalg.inv(S)
self.x = self.x + K @ y
self.P = (np.eye(len(self.x)) - K @ self.H) @ self.P
b. UKF滤波
import numpy as np
from scipy.linalg import sqrtm
class UKF:
def __init__(self, f, h, Q, R, P, x0):
self.f = f
self.h = h
self.Q = Q
self.R = R
self.P = P
self.x = x0
self.n = x0.shape[0]
self.m = None
def predict(self):
# Generate sigma points
L = np.linalg.cholesky(self.P)
W = np.concatenate((np.zeros((self.n, 1)), sqrtm(self.n * self.P)))
X = np.concatenate((self.x, self.x + W, self.x - W), axis=1)
# Predict sigma points
Y = np.zeros((self.n, 2 * self.n + 1))
for i in range(2 * self.n + 1):
Y[:, i] = self.f(X[:, i])
# Compute mean and covariance
self.x = np.mean(Y, axis=1, keepdims=True)
self.P = np.cov(Y) + self.Q
def update(self, z):
# Generate sigma points
L = np.linalg.cholesky(self.P)
W = np.concatenate((np.zeros((self.n, 1)), sqrtm(self.n * self.P)))
X = np.concatenate((self.x, self.x + W, self.x - W), axis=1)
# Predict measurements
Z = np.zeros((self.m, 2 * self.n + 1))
for i in range(2 * self.n + 1):
Z[:, i] = self.h(X[:, i])
# Compute mean and covariance
z_mean = np.mean(Z, axis=1, keepdims=True)
z_cov = np.cov(Z) + self.R
# Compute cross-covariance
xz_cov = np.zeros((self.n, self.m))
for i in range(2 * self.n + 1):
xz_cov += (X[:, i, np.newaxis] - self.x) @ (Z[:, i, np.newaxis] - z_mean).T
xz_cov /= 2 * self.n
# Compute Kalman gain
K = xz_cov @ np.linalg.inv(z_cov)
# Update estimate
self.x += K @ (z - z_mean)
self.P -= K @ z_cov @ K.T
第一个版本
第二个是我自己改的一个。参考MATLAB的流程，直接改成了python代码，没有做代码的优化，结果还挺好的，和MATLAB结果一致。
import math
import numpy as np
from scipy.linalg import sqrtm
class ukf:
def __init__(self, f, h):
self.f = f
self.h = h
self.Q = None
self.R = None
self.P = None
self.x = None
self.Z = None
self.n = None
self.m = None
def GetParameter(self, Q, R, P, x0):
self.Q = Q
self.R = R
self.P = P
self.x = x0
self.n = x0.shape[0]
self.m = None
def sigmas(self,x0, c):
A = c * np.linalg.cholesky(self.P).T
Y = (self.x * np.ones((self.n,self.n))).T
Xset = np.concatenate((x0.reshape((-1,1)), Y+A, Y-A), axis=1)
return Xset
def ut1(self, Xsigma, Wm, Wc):
LL = Xsigma.shape[1]
Xmeans = np.zeros((self.n,1))
Xsigma_pre = np.zeros((self.n, LL))
for k in range(LL):
Xsigma_pre[:,k] = self.f(Xsigma[:,k])
Xmeans = Xmeans + Wm[0,k]* Xsigma_pre[:, k].reshape((self.n, 1))
Xdiv = Xsigma_pre - np.tile(Xmeans,(1,LL))
P = np.dot(np.dot(Xdiv, np.diag(Wc.reshape((LL,)))), Xdiv.T) + self.Q
return Xmeans, Xsigma_pre, P, Xdiv
def ut2(self, Xsigma, Wm, Wc, m):
LL = Xsigma.shape[1]
Xmeans = np.zeros((m, 1))
Xsigma_pre = np.zeros((m, LL))
for k in range(LL):
Xsigma_pre[:, k] = self.h(Xsigma[:, k])
Xmeans = Xmeans + Wm[0, k] * Xsigma_pre[:, k].reshape((m, 1))
Xdiv = Xsigma_pre - np.tile(Xmeans, (1, LL))
P = np.dot(np.dot(Xdiv, np.diag(Wc.reshape((LL,)))), Xdiv.T) + self.R
return Xmeans, Xsigma_pre, P, Xdiv
def OutPutParameter(self, alpha_msm, x0, Q, R, P):
z = np.array(alpha_msm).reshape((3, 1))
self.GetParameter(Q, R, P, x0)
l = self.n
m = z.shape[0]
alpha = 2
ki = 3 - l
beta = 2
lamb = alpha ** 2 * (l + ki) - l
c = l + lamb
Wm = np.concatenate((np.array(lamb / c).reshape((-1,1)), 0.5 / c + np.zeros((1, 2 * l))), axis=1)
Wc = Wm.copy()
Wc[0][0] = Wc[0][0] + (1 - alpha ** 2 + beta)
c = math.sqrt(c)
Xsigmaset = self.sigmas(x0, c)
X1means, X1, P1, X2 = self.ut1(Xsigmaset, Wm, Wc)
Zpre, Z1, Pzz, Z2 = self.ut2(X1, Wm, Wc, m)
Pxz = np.dot(np.dot(X2 , np.diag(Wc.reshape((self.n*2+1,)))), Z2.T)
K =np.dot(Pxz , np.linalg.inv(Pzz))
X = (X1means + np.dot(K, z - Zpre)).reshape((-1,))
self.P = P1 - np.dot(K , Pxz.T)
return X, self.P
这里把两个代码都公开出来，以供参考。
如有疑问，欢迎提问和指正。
