文章目录

• 简介
• 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 = 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

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

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

查看全文

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.dgrt.cn/a/1798461.html

如若内容造成侵权/违法违规/事实不符，请联系一条长河网进行投诉反馈，一经查实，立即删除！

相关文章：

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

文章目录简介UKF滤波1. 概述和流程2. Python代码第一个版本a. KF滤波b. UKF滤波第一个版本简介
上一篇文章，我们介绍了UKF滤波公式及其MATLAB代码。在做视觉测量的过程中，基于OpenCV的开发包比较多，因此我们将UKF的MATLAB代码转到python中&a……

编程日记2023/3/10 19:25:28

【程序语言】程序循环的那些事儿

我一直认为循环是程序的发动机，所以决定深入探究一下循环的那些事儿！ 下面我们研究一下循环的写法和效率： 背景是这假设有一个存放20个元素的数组int arr[20]，我们想对它进行求和，也就arr[0]arr[1]…… 好的&#xff0……

编程日记2023/3/10 19:25:26

【程序语言】元编程带来的代码展开技巧

我们讨论过对int arr[20]所有元素求和的最高执行效率代码，那就是： int sum arr[0] arr[1] arr[2] arr[3]arr[4] arr[5] arr[6] arr[7]arr[8] arr[9] arr[10] arr[11]arr[12] arr[13] arr[14] arr[15]arr[16] arr[17] arr[18] arr[19];
但是这……

编程日记2023/3/10 19:25:26

【程序语言】C++的扩充进化之路

编程语言之于程序员就如剑之于剑客，剑客不会因为剑而天下无敌，但每个剑客有应有自己心爱的剑……
而我所喜爱的剑就是C，也许每个江湖中的剑客都会对剑有爱有恨，我对C的感觉也是“爱之深，责之切”
不说那些讨厌C或者喜……

编程日记2023/3/10 19:25:25

【程序语言】C++中的执行时间测量

测试一段代码，或者一个函数是写完代码后经常要做的事。
我习惯的写东西的顺序是：
测试代码 –> 伪代码 –> 实际代码 –> 更加高效的版本 –> 带输入输出控制检测，出错验证的代码 –>更加灵活的版本
下面列举下常用的时间……

编程日记2023/3/10 19:25:24

【程序语言】C++ 构造函数 拷贝构造函数 =操作符重载 析构函数 详细分析

当你满怀轻松得心情写下一个简单的C类：
class MyClass{private:int a;}你心里也许十分高兴，因为你恐怕找不到更简单的类了，写简单的东西总是轻松越快的。但是C却附赠了些东西给你：
public:MyClass() {/* ……

编程日记2023/3/10 19:25:24

【网站架构】高性能网站架构

________________________________已废止文章，留存仅作纪念，其中观点尚属幼稚——————————————————————————
——————————————————————————————————————————————————————……

编程日记2023/3/10 19:25:23

【算法设计与分析】5个数7次比较排序的算法

找到最快的算法，一直是计算机界的目标之一，而排序就是其中最基本的算法。什么样的排序才是最快的呢？
1.最少的比较次数，算法理论证明n个数排序，如果是基于比较的算法，至少需要 ㏒(n!) 向上取整数。下面给出……

编程日记2023/3/10 19:25:22

【程序语言】C/C++中如何使用Lua脚本

Lua作为一门优雅高效的脚本语言，开始受到越来越多的关注。很多对Lua感兴趣的朋友最郁闷的问题就是如何开始入手。那么现在我就也以一个初学者的身份，带大家一步一步开始Lua之旅：
1.确保有一个可用的编程环境，我这里示范的是VS201……

编程日记2023/3/10 19:25:22

我们【即将面对】和【已经面对】的那些事儿

计算机世界在以一种令人吃惊的速度前进着，从1946年到现在，也不过六十余年的光阴。与动则上百年到千年历史的其他学科相比，计算机科学是那么的年轻。但正是这门科学飞速地改变着我们的生活。理论和实践都在继续推进着，而我们也开始……

编程日记2023/3/10 19:25:21

【hello Linux】环境变量

目录 1. 环境变量的概念 2. 常见的环境变量 3. 查看环境变量 4. 和环境变量相关的命令 5. 环境变量的组织方式 6. 通过代码获取环境变量 7. 通过系统调用获取环境变量 Linux🌷 在开始今天的内容之前，先来看一幅图片吧！ 不知道你们是否和我一……

编程日记2023/4/16 14:51:36

【Linux基础】常用命令整理

ls命令
-a选项，可以展示隐藏的文件和文件夹-l选项，以列表形式展示内容-h，需要和-l搭配使用，可以展示文件的大小单位ls -lah等同于la -a -l -h
cd命令（change directory）
语法：cd [Linux路径]……

编程日记2023/4/16 14:51:26

客快物流大数据项目（一百一十二）：初识Spring Cloud

文章目录
初识Spring Cloud
一、Spring Cloud简介
二、SpringCloud 基础架构图…

编程日记2023/4/16 14:51:11

C和C++中的struct有什么区别

区别一： C语言中： Struct是用户自定义数据类型（UDT）。 C语言中： Struct是抽象数据类型（ADT），支持成员函数的定义。
区别二：
C中的struct是没有权限设置的&#xff0c……

编程日记2023/4/16 14:50:59

docker的数据卷详解

数据卷 数据卷是宿主机中的一个目录或文件，当容器目录和数据卷目录绑定后，对方修改会立即同步
一个数据卷可以同时被多个容器同时挂载，一个容器也可以被挂载多个数据卷
数据卷作用：容器数据持久化 /外部机器和容器间接通信 /容器……

编程日记2023/4/16 14:50:50

13、Qt生成dll-QLibrary方式使用

Qt创建dll，使用QLibrary类方式调用dll
一、创建项目
1、新建项目->其他项目->Empty qmake Project->Choose 2、输入项目名，选择项目位置，下一步 3、选择MinGW，下一步 4、完成 5、.pro中添加TEMPLATE subdirs&#xff……

编程日记2023/4/16 14:50:38

基于mapreduce 的 minHash 矩阵压缩

Minhash作用： 对大矩阵进行降维处理，在进行计算俩个用户之间的相似度。
比如： 俩个用户手机下载的APP的相似度，在一个矩阵中会有很多很多的用户要比较没俩个用户之间的相似度是一个很大的计算任务 如果首先对这个矩阵降维处理&am……

编程日记2023/4/16 14:50:37

关于hashmap使用迭代器的问题

keySet获得的只是key值的集合，valueSet获得的是value集合，entryset获得的是键值对的集合。 package com.test2.test;import java.util.HashMap;
import java.util.Iterator;
import java.util.Map;
import java.util.Map.Entry;public class mapiterator……

编程日记2023/4/16 14:50:37

Hadoop入口FileSystem HDFS操作 本地文件合并到HDFS和HDFS文件合并

Hadoop 文件API的起点是FileSystem类。这是一个与文件系统交互的抽象类。存在不同的具体实现子类来处理HDFS和本地文件系统。
HDFS接口的FileSystem对象：
Configuration conf new Configuration();
FileSystem hdfs FileSystem.get(conf); HDFS直接操作&#x……

编程日记2023/4/16 14:50:36

combiner partitioner

combine是在map端进行的，是在patition之后 partitioner也是在map端进行的 combine 适用在每个map端进行简单的合并，同样也是继承Reduce类。…

编程日记2023/4/16 14:50:35