如何提升网站的流量,网站从建设到运行要多少,建设厅安全证考试报名在哪个网站,公关公司都有哪些职位文章目录简介UKF滤波1. 概述和流程2. Python代码第一个版本a. KF滤波b. UKF滤波第二个版本简介
上一篇文章#xff0c;我们介绍了UKF滤波公式及其MATLAB代码。在做视觉测量的过程中#xff0c;基于OpenCV的开发包比较多#xff0c;因此我们将UKF的MATLAB代码转到python中我们介绍了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 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.Pb. 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), axis1)# 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, axis1, keepdimsTrue)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), axis1)# 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, axis1, keepdimsTrue)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)), YA, Y-A), axis1)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))), axis1)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*21,)))), 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
这里把两个代码都公开出来以供参考。
如有疑问欢迎提问和指正。
