滤波算法 | 无迹卡尔曼滤波（UKF）算法及其Python实现

简介

上一篇文章，我们介绍了UKF滤波公式及其MATLAB代码。在做视觉测量的过程中，基于OpenCV的开发包比较多，因此我们将UKF的MATLAB代码转到python中，实现数据滤波效果。

UKF滤波

1. 概述和流程

UKF的公式这里就不再过多介绍了，具体内容请参见博客：UKF滤波公式及其MATLAB代码

这里简单把上一篇文章的公式和流程图粘贴一下。

求解流程 ：

1. 相比于一般的卡尔曼滤波，UKF算法增加了两次无迹变换，公式为：
   
   权重和方差计算公式为：
   

2. Sigma点传播：

3. 计算x的预测值和协方差矩阵：

4. 得到一组新的Sigma点：

5. 代入观测方程中，得到测量量的预估值：

6. 获得观测量的预测值和协方差矩阵：

7. 更新状态变量和协方差矩阵：
   
   
   

另外，每次写论文画卡尔曼流程图中，都找不到参考的模板。我自己画了个滤波流程图，不一定符合每个人的审美，以备参考：

2. Python代码

重点来了。。。

上代码。

第一个版本

UKF的python代码我一共写了两个版本。

第一个是我用ChatGPT直接生成了一个，经过数据实测，结果有点奇怪，不太像我想要的结果，每个模块之间的交互也跟我理解的不太一样。为了对比，这里也还是贴出来了，人家写的确实比我写的看着逼格好点。

ChatGPT输出的Python：

a. KF滤波

class KalmanFilter:def __init__(self, F, H, Q, R, P, x0):self.F = Fself.H = Hself.Q = Qself.R = Rself.P = Pself.x = x0def predict(self):self.x = self.F @ self.xself.P = self.F @ self.P @ self.F.T + self.Qdef update(self, z):y = z - self.H @ self.xS = self.H @ self.P @ self.H.T + self.RK = self.P @ self.H.T @ np.linalg.inv(S)self.x = self.x + K @ yself.P = (np.eye(len(self.x)) - K @ self.H) @ self.P

b. UKF滤波

import numpy as np
from scipy.linalg import sqrtmclass UKF:def __init__(self, f, h, Q, R, P, x0):self.f = fself.h = hself.Q = Qself.R = Rself.P = Pself.x = x0self.n = x0.shape[0]self.m = Nonedef predict(self):# Generate sigma pointsL = 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 pointsY = 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 covarianceself.x = np.mean(Y, axis=1, keepdims=True)self.P = np.cov(Y) + self.Qdef update(self, z):# Generate sigma pointsL = 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 measurementsZ = 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 covariancez_mean = np.mean(Z, axis=1, keepdims=True)z_cov = np.cov(Z) + self.R# Compute cross-covariancexz_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).Txz_cov /= 2 * self.n# Compute Kalman gainK = xz_cov @ np.linalg.inv(z_cov)# Update estimateself.x += K @ (z - z_mean)self.P -= K @ z_cov @ K.T

第二个版本

第二个是我自己改的一个。参考MATLAB的流程，直接改成了python代码，没有做代码的优化，结果还挺好的，和MATLAB结果一致。

import mathimport numpy as np
from scipy.linalg import sqrtmclass ukf:def __init__(self, f, h):self.f = fself.h = hself.Q = Noneself.R = Noneself.P = Noneself.x = Noneself.Z = Noneself.n = Noneself.m = Nonedef GetParameter(self, Q, R, P, x0):self.Q = Qself.R = Rself.P = Pself.x = x0self.n = x0.shape[0]self.m = Nonedef sigmas(self,x0, c):A = c * np.linalg.cholesky(self.P).TY = (self.x * np.ones((self.n,self.n))).TXset = np.concatenate((x0.reshape((-1,1)), Y+A, Y-A), axis=1)return Xsetdef 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.Qreturn Xmeans, Xsigma_pre, P, Xdivdef 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.Rreturn Xmeans, Xsigma_pre, P, Xdivdef OutPutParameter(self, alpha_msm, x0, Q, R, P):z = np.array(alpha_msm).reshape((3, 1))self.GetParameter(Q, R, P, x0)l = self.nm = z.shape[0]alpha = 2ki = 3 - lbeta = 2lamb = alpha ** 2 * (l + ki) - lc = l + lambWm = 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

这里把两个代码都公开出来，以供参考。

如有疑问，欢迎提问和指正。