利用Python创建Kalman滤波器用于多普勒测速目标跟踪

2024-03-06 02:40

本文主要是介绍利用Python创建Kalman滤波器用于多普勒测速目标跟踪,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!

利用Python构造Kalman滤波器和拓展卡尔曼滤波器Class用于目标跟踪

文章目录

    • 利用Python构造Kalman滤波器和拓展卡尔曼滤波器Class用于目标跟踪
      • Kalman滤波概述
      • Kalman滤波器Python类
      • 线性动力学、线性观测
      • 所有代码

Kalman滤波概述

其基本思路是:

  • 新的最佳估计基于原最佳估计已知外部影响校正后得到的预测。
  • 新的不确定性基于原不确定性外部环境的不确定性得到的预测。

对于系统已知以下离散系统方程、线性观测方程:
x k + 1 = Φ k + 1 , k x k + Ψ k + 1 , k u k + Γ k w k z k + 1 = H k + 1 x k + 1 + v k + 1 \begin{aligned} &x_{k+1}=\Phi_{k+1, k} x_{k}+\Psi_{k+1, k} u_{k}+\Gamma_{k} w_{k} \\ &z_{k+1}=H_{k+1} x_{k+1}+v_{k+1} \end{aligned} xk+1=Φk+1,kxk+Ψk+1,kuk+Γkwkzk+1=Hk+1xk+1+vk+1Kalman滤波的基本公式如下:
一步预测状态 ⋯ x ^ k + 1 ∣ k = Φ k + 1 , k x ^ k ∣ k + Ψ k + 1 , k u k 预测系统协方差矩阵 ⋯ P k + 1 ∣ k = Φ k + 1 , k P k ∣ k Φ k + 1 , k T + Γ k Q k Γ k T Kalman增益计算 ⋯ K k + 1 = P k + 1 ∣ k H k + 1 T [ H k + 1 P k + 1 ∣ k H k + 1 T + R k + 1 ] − 1 修正后的估计状态 ⋯ x ^ k + 1 ∣ k + 1 = x ^ k + 1 ∣ k + K k + 1 [ z k + 1 − H k + 1 x ^ k + 1 ∣ k ] 状态误差最优估计 ⋯ P k + 1 ∣ k + 1 = [ I − K k + 1 H k + 1 ] P k + 1 ∣ k \begin{aligned} \texttt{一步预测状态}\cdots \hat{x}_{k+1 \mid k}=&\Phi_{k+1, k} \hat{x}_{k \mid k}+\Psi_{k+1, k} u_{k} \\ \texttt{预测系统协方差矩阵}\cdots P_{k+1 \mid k}=&\Phi_{k+1, k} P_{k \mid k} \Phi_{k+1, k}^{\mathrm{T}}+\Gamma_{k} Q_{k} \Gamma_{k}^{\mathrm{T}} \\ \texttt{Kalman增益计算}\cdots K_{k+1}=&P_{k+1 \mid k} H_{k+1}^{\mathrm{T}}\left[H_{k+1} P_{k+1 \mid k} H_{k+1}^{T}+R_{k+1}\right]^{-1} \\ \texttt{修正后的估计状态}\cdots \hat{x}_{k+1 \mid k+1}=&\hat{x}_{k+1 \mid k}+K_{k+1}\left[z_{k+1}-H_{k+1} \hat{x}_{k+1 \mid k}\right] \\ \texttt{状态误差最优估计}\cdots P_{k+1 \mid k+1}=&\left[I-K_{k+1} H_{k+1}\right] P_{k+1 \mid k} \end{aligned} 一步预测状态x^k+1k=预测系统协方差矩阵Pk+1k=Kalman增益计算Kk+1=修正后的估计状态x^k+1k+1=状态误差最优估计Pk+1k+1=Φk+1,kx^kk+Ψk+1,kukΦk+1,kPkkΦk+1,kT+ΓkQkΓkTPk+1kHk+1T[Hk+1Pk+1kHk+1T+Rk+1]1x^k+1k+Kk+1[zk+1Hk+1x^k+1k][IKk+1Hk+1]Pk+1k式中:

符号解释
x ^ k + 1 / k \hat x_{k+1/k} x^k+1/k根据 k k k时刻状态和系统方程预测的(先验)值
x ^ k + 1 / k + 1 \hat x_{k+1/k+1} x^k+1/k+1Kalman滤波得到的 k + 1 k+1 k+1时刻,也就是综合了以前结果和当前这一步观测值,所得到的状态估计值
x k + 1 x_{k+1} xk+1或者 x k + 1 / k + 1 x_{k+1/k+1} xk+1/k+1 k + 1 k+1 k+1时刻真实的状态
z ^ k + 1 / k \hat z_{k+1/k} z^k+1/k根据预测状态 x k + 1 / k x_{k+1/k} xk+1/k,对观测结果的预测
z k + 1 z_{k+1} zk+1 k + 1 k+1 k+1时刻传感器的测量值
z k + 1 − H x ^ k + 1 / k z_{k+1}-H\hat x_{k+1/k} zk+1Hx^k+1/k预测的和实际测量值之间的差距
P k + 1 / k P_{k+1/k} Pk+1/k预测值和真实值之间误差的协方差矩阵
P k + 1 / k + 1 P_{k+1/k+1} Pk+1/k+1下一步 k + 1 k+1 k+1估计值和真实值之间的误差协方差矩阵
K k K_k Kkkalman增益

状态协方差矩阵定义为 P k = E [ e k e k T ] = E [ ( x k − x ^ k ) ( x k − x ^ k ) T ] P_{k}=E\left[e_{k} e_{k}^{T}\right]=E\left[\left(x_{k}-\hat{x}_{k}\right)\left(x_{k}-\hat{x}_{k}\right)^{T}\right] Pk=E[ekekT]=E[(xkx^k)(xkx^k)T],代表估计值和真实值之间的误差。滤波时,可以估计出状态协方差的初值 P 0 P_0 P0(迭代更新),测量噪声协方差 R = Cov ⁡ ( w 1 ) R = \operatorname{Cov}(w_1) R=Cov(w1)、系统噪声协方差矩阵 Q = Cov ⁡ ( [ v 1 , v 2 ] ) Q =\operatorname{Cov}([v_1,v_2]) Q=Cov([v1,v2]),以及初值 x ^ 0 ∣ 0 = E x 0 = m 0 \hat{x}_{0 \mid 0}=E x_{0}=m_{0} x^00=Ex0=m0

关于这几个矩阵的设定,可见过客冲冲 - 理解Kalman滤波的使用 cnblogs。

P是误差矩阵,初始化可以是一个随机的矩阵或者0,只要经过几次的处理基本上就能调整到正常的水平,因此也就只会影响前面几次的滤波结果。
Q和R分别是预测和观测状态协方差矩阵,一般可以简单认为系统状态各维之间(即上面的a和b)相互独立,那么Q和R就可以设置为对角阵。而这两个对角线元素的大小将直接影响着滤波结果,若Q的元素远大于R的元素,则预测噪声大,从而更相信观测值,这样可能使得kalman滤波结果与观测值基本一致;反之,则更相信预测,kalman滤波结果会表现得比较规整和平滑;若二者接近,则滤波结果介于前面两者之间,根据实验效果看也缺乏实际使用价值。

Kalman滤波器Python类

下面这个是根据上图中的公式写的,简单改一下就是EKF,参考360doc 时钟转动 / 卡尔曼滤波 / Kalman滤波器的C++实现

import numpy as np
import math
# import scipy.stats as st
# import scipy
# import matplotlib as mpl
import matplotlib.pyplot as pltclass KalmanFilter:stateSize = 1measureSize = 1controlSize = 0IsNonLinearObserve , funHz = (0, '')x,z,A,B,G,u,Px,K,Hz,Qv,Rm = [0,0,0,0,0,0, 0,0,0,0,0]def __init__(self,n,m,nc):self.stateSize = nself.measureSize = mself.controlSize = ncself.x = np.zeros([n,1])self.z = np.zeros([m,1])self.In = np.eye(n)self.A = np.eye(n)         # state transition matrix, discreteif nc>0:self.u = np.zeros([nc,1])self.B = np.zeros(n,nc)self.G = np.eye(n)self.Px = np.eye(n)*1e-6self.Qv = 1e-6*np.eye(n)  # process varianceself.K  = np.zeros([n,m]) # kalman gainself.Hz = np.zeros([m,n])self.Rm = 1e-6*np.eye(m)  # estimate of measurement variance,def init(self,x_,P_,Q_,R_):for i in range(0,self.stateSize):self.x[i,:] = x_[i,:]self.Px = P_self.Qv = Q_self.Rm = R_return selfdef predict(self,A_):self.A = A_self.x = self.A@self.x
#         print(self.x)self.P = self.A@self.Px@self.A.T + self.G@self.Qv@self.G.T return selfdef observe(self,x):if self.IsNonLinearObserve:# 这是非线性观测方程z = self.funHz(x)z_predict = np.array( [ [z[0]], [z[1]] ] )else:z_predict = self.Hz@xreturn z_predictdef update(self,H_,z_):self.Hz = H_Ht = self.Hz.Ttemp1 = self.Hz@self.P@Ht + self.Rm
#         temp2 = temp1.inverse() #(H*P*H'+R)^(-1)self.K = self.P@Ht@np.linalg.inv(temp1)self.z = self.observe(self.x)# 根据方程类型调用不同的观测函数self.x = self.x + self.K@(z_ - self.z)
#         print(self.x)self.P = (self.In - self.K@self.Hz)@self.Pxreturn self

线性动力学、线性观测

以下是一个一维Doppler测速估计目标状态的模型:
x ˙ = A x + ν = [ r ˙ v ˙ ] = [ 0 1 0 0 ] [ r v ] + [ v 1 v 2 ] z = H x + w = [ 0 1 ] [ r v ] + w 1 \begin{aligned} &\dot{x}=Ax+\nu =\left[\begin{array}{l} \dot{r} \\ \dot{v} \end{array}\right]=\left[\begin{array}{ll} 0 & 1 \\ 0 & 0 \end{array}\right]\left[\begin{array}{l} r \\ v \end{array}\right]+\left[\begin{array}{ll} v_{1}\\ v_{2} \end{array}\right] \\ &z = Hx +w=\left[\begin{array}{ll} 0 & 1 \end{array}\right]\left[\begin{array}{r} r \\ v \end{array}\right]+w_1 \end{aligned} x˙=Ax+ν=[r˙v˙]=[0010][rv]+[v1v2]z=Hx+w=[01][rv]+w1然后协方差矩阵定义为: Cov ⁡ ( x ^ − x ) = P , Cov ⁡ ( [ v 1 , v 2 ] ) = Q , Cov ⁡ ( w 1 ) = R \operatorname{Cov}(\hat{x}-x)=P \ ,\ \operatorname{Cov}([v_1,v_2])=Q \ ,\ \operatorname{Cov}(w_1)=R Cov(x^x)=P , Cov([v1,v2])=Q , Cov(w1)=R目标初始时刻 x ( 0 ) ≈ 0 x(0) \approx 0 x(0)0, 已知目标是匀速直线运动,要求用KF估计其位置和速度。

初始化动力学系统:

# # 
# SEC 2, SYSTEM# 
# # 
def calExpAt(A,dt):n = len(A)I = np.eye(n)return I+A*dt+A@A/2*dt*dt#########################################################################
# 系统设置,初值估计
#########################################################################
measureStep = 0.1;
tf = 30;
lth = 1 + int(tf/measureStep);
ts = np.linspace(0,tf,lth)A = np.array([[0,1.0],[0,0]])           # system matrix
H = np.array([[0,1.0]])                 # 注意sizex0 = np.array([0,0.8])                    # 我这里随意设置了一个初值,与真实值相差较大
x0 = x0[:,np.newaxis]
# x0 = np.array([[x1],[x2]]) 
#########################################################################
# 先验统计误差,
#########################################################################
sigmaQv = np.array([0.1,0.1])                     # estimated pertubation of dynamical system per step# 如果第一步的初始状态估计误差就很大,这里就需要设置稍大;否则,收敛将很慢
Q = np.eye(2)
Q[0,0] = Q[0,0]*sigmaQv[0]**2           # pertubation from white noise, may be from 
Q[1,1] = Q[1,1]*sigmaQv[1]**2
sigmaRm = 5                           # a priori precision of measurement
R = np.array([[sigmaRm**2]])            # covariance of measurement precisionP0 = np.eye(2)                          # error of state, estimated at start
P0[0,0] = 1*P0[0,0]
P0[1,1] = 3*P0[1,1]                                #########################################################################
# 采用先验误差设定,模拟真实的、带有噪声的测量数据
#########################################################################
# z_ts = np.linspace(0,18,lth)+np.random.normal(0,sigmaRm,lth)  # 匀加速
v = 1.2
z_real = v*np.ones((lth))               # 一维数组,尚未扩充维度到2
z_ts = z_real+np.random.normal(0,sigmaQv[1]*sigmaQv[1],lth)
z_ts = z_ts[np.newaxis, :]              # 作为行向量输入

创建Kalman滤波器并滤波,获得目标的状态:


# # 
# SEC 3, ESTIMATE/# 
# # 
# 创建并初始化
dopp = KalmanFilter(2,1,0)
# 设置先验误差矩阵
dopp.init(x0,P0,Q,R)# 保存数据
# P_ts = P[:,:,np.newaxis]
x_ts = np.zeros([lth,2])for it in range(0,lth):dopp.predict(calExpAt(A,measureStep))dopp.update(H,z_ts[:,it,np.newaxis])x_ts[it,:] = dopp.x.T[0]# x_ts[0,it] = dopp.x[0],x_ts[1,it] = dopp.x[1]# 绘图
plt.subplot(2,1,1)
plt.plot(ts,x_ts[:,1])
plt.plot(ts,z_ts[0])
plt.legend(['filtered data', 'measure data'])
plt.title('Doppler velocity estimate result')plt.subplot(2,1,2)
plt.plot(ts,x_ts[:,1]-z_real)
plt.xlabel('t/[s]')
plt.title('estimatation error')
plt.show()

以下是两个设定的例子, 估计速度初值为 0.8 m / s 0.8m/s 0.8m/s,实际速度 1.2 m / s 1.2m/s 1.2m/s;初始位置为 0 0 0,协方差矩阵设定不同:
Q = diag ⁡ ( 0.1 , 0.1 ) 2 , R = diag ⁡ ( 5.0 ) 2 , P 0 = diag ⁡ ( 3 , 3 ) Q=\operatorname{diag}(0.1,0.1)^2,R=\operatorname{diag}(5.0)^2,P_0=\operatorname{diag}(3,3) Q=diag(0.1,0.1)2,R=diag(5.0)2,P0=diag(3,3)请添加图片描述
Q = diag ⁡ ( 1.0 , 0.1 ) 2 , R = diag ⁡ ( 0.2 ) 2 , P 0 = diag ⁡ ( 3 , 3 ) Q=\operatorname{diag}(1.0,0.1)^2,R=\operatorname{diag}(0.2)^2,P_0=\operatorname{diag}(3,3) Q=diag(1.0,0.1)2,R=diag(0.2)2,P0=diag(3,3)
请添加图片描述

按照误差标准差公式 S = ∑ i = 1 n ( x i − x ˉ ) 2 n − 1 S=\sqrt{\frac{\sum_{i=1}^{n}\left(x_{i}-\bar{x}\right)^{2}}{n-1}} S=n1i=1n(xixˉ)2 统计误差

# 统计一下估计误差
v_hat = x_ts[-100:,1]
err = v_hat-z_real[-100:]
std = np.sum((err- np.mean(err))**2)/99
print('estimated covariance of navigation: ',math.sqrt(std))

所有代码

限于篇幅,这里给出剩下的内容,包括一个非线性观测、非线性系统情况下的拓展卡尔曼滤波案例。
Simple Extended Kalman Filter for target tracking

这篇关于利用Python创建Kalman滤波器用于多普勒测速目标跟踪的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!


原文地址:https://blog.csdn.net/NICAI001/article/details/120154398
本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.chinasem.cn/article/778628

相关文章

Python实现批量提取BLF文件时间戳

《Python实现批量提取BLF文件时间戳》BLF(BinaryLoggingFormat)作为Vector公司推出的CAN总线数据记录格式,被广泛用于存储车辆通信数据,本文将使用Python轻松提取... 目录一、为什么需要批量处理 BLF 文件二、核心代码解析:从文件遍历到数据导出1. 环境准备与依赖库

Python Web框架Flask、Streamlit、FastAPI示例详解

《PythonWeb框架Flask、Streamlit、FastAPI示例详解》本文对比分析了Flask、Streamlit和FastAPI三大PythonWeb框架:Flask轻量灵活适合传统应用... 目录概述Flask详解Flask简介安装和基础配置核心概念路由和视图模板系统数据库集成实际示例Stre

Python实现PDF按页分割的技术指南

《Python实现PDF按页分割的技术指南》PDF文件处理是日常工作中的常见需求,特别是当我们需要将大型PDF文档拆分为多个部分时,下面我们就来看看如何使用Python创建一个灵活的PDF分割工具吧... 目录需求分析技术方案工具选择安装依赖完整代码实现使用说明基本用法示例命令输出示例技术亮点实际应用场景扩

Python错误AttributeError: 'NoneType' object has no attribute问题的彻底解决方法

《Python错误AttributeError:NoneTypeobjecthasnoattribute问题的彻底解决方法》在Python项目开发和调试过程中,经常会碰到这样一个异常信息... 目录问题背景与概述错误解读:AttributeError: 'NoneType' object has no at

Python使用openpyxl读取Excel的操作详解

《Python使用openpyxl读取Excel的操作详解》本文介绍了使用Python的openpyxl库进行Excel文件的创建、读写、数据操作、工作簿与工作表管理,包括创建工作簿、加载工作簿、操作... 目录1 概述1.1 图示1.2 安装第三方库2 工作簿 workbook2.1 创建:Workboo

基于Python实现简易视频剪辑工具

《基于Python实现简易视频剪辑工具》这篇文章主要为大家详细介绍了如何用Python打造一个功能完备的简易视频剪辑工具,包括视频文件导入与格式转换,基础剪辑操作,音频处理等功能,感兴趣的小伙伴可以了... 目录一、技术选型与环境搭建二、核心功能模块实现1. 视频基础操作2. 音频处理3. 特效与转场三、高

Python实现中文文本处理与分析程序的示例详解

《Python实现中文文本处理与分析程序的示例详解》在当今信息爆炸的时代,文本数据的处理与分析成为了数据科学领域的重要课题,本文将使用Python开发一款基于Python的中文文本处理与分析程序,希望... 目录一、程序概述二、主要功能解析2.1 文件操作2.2 基础分析2.3 高级分析2.4 可视化2.5

一文解密Python进行监控进程的黑科技

《一文解密Python进行监控进程的黑科技》在计算机系统管理和应用性能优化中,监控进程的CPU、内存和IO使用率是非常重要的任务,下面我们就来讲讲如何Python写一个简单使用的监控进程的工具吧... 目录准备工作监控CPU使用率监控内存使用率监控IO使用率小工具代码整合在计算机系统管理和应用性能优化中,监

Python实现终端清屏的几种方式详解

《Python实现终端清屏的几种方式详解》在使用Python进行终端交互式编程时,我们经常需要清空当前终端屏幕的内容,本文为大家整理了几种常见的实现方法,有需要的小伙伴可以参考下... 目录方法一:使用 `os` 模块调用系统命令方法二:使用 `subprocess` 模块执行命令方法三:打印多个换行符模拟

Python实现MQTT通信的示例代码

《Python实现MQTT通信的示例代码》本文主要介绍了Python实现MQTT通信的示例代码,文中通过示例代码介绍的非常详细,对大家的学习或者工作具有一定的参考学习价值,需要的朋友们下面随着小编来一... 目录1. 安装paho-mqtt库‌2. 搭建MQTT代理服务器(Broker)‌‌3. pytho