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

Posted 2023-03-12 lovetaozibaby

文章目录

• 简介
• UKF滤波
• • 1. 概述和流程
  • 2. Python代码
  • • 第一个版本
    • • a. KF滤波
      • b. UKF滤波
    • 第二个版本

简介

上一篇文章，我们介绍了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 = 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

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

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