✅作者简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，代码获取、论文复现及科研仿真合作可私信。
🍎个人主页：Matlab科研工作室
🍊个人信条：格物致知。
更多Matlab完整代码及仿真定制内容点击👇
🔥 内容介绍
标准卡尔曼滤波方法是建立在线性系统和高斯分布两个假设条件之上的,然而绝大多数实际系统都存在一定程度的非线性,使得原本符合高斯分布的随机变量经非线性变换后不再满足高斯特性,导致卡尔曼滤波无法直接应用.
为解决非线性系统的滤波问题,将非线性系统进行线性化是最直接的做法,从而将非线性滤波问题转化为线性滤波问题.线性化是将非线性函数进行 Taylor展开或者以多项式的方式进行拟合,然后忽略高阶小项得到偏差线性化函数的过程,根据所取阶数不同可以获得不同的近似精度,一般以多阶线性化获得较高近似精度的,相应的计算量也会比较大.
EKF是一种比较简单的将非线性方程进行Taylor一阶展开并忽略高阶项的扩展卡尔曼滤波方法,由于其该方法只是一阶近似,因此当系统非线性较强时会带来较大的截断误差.但由于EKF方法的小计算复杂度带来的简便性,在工程中,对于一些非线性并不强的系统仍被广泛使用.
一阶扩展卡尔曼滤波方法
对于以下非线性系统:
式中, $f(\cdot)$ 和 $h(\cdot)$ 为非线性方程; $w_{k-1}$ 和 $v_k$ 为互不相关的高斯高斯白噪声,其噪声统计特性为:
式中, $Q_k$ 为过程噪声方差矩阵; $R_k$ 为量测噪声方差阵.
将非线性状态方程 $f(\cdot)$ 在状态估计值 $\hat{x}_{k-1}$ 附近展成一阶Taylor级数,可得
式中,
同理,将非线性量测方程 $h_k(\cdot)$ 在状态估计值 $\hat{x}_{k|k-1}$ 附近展开成一阶Taylor级数,可得
式中,
令则非线性量测方程的一阶线性化结果为:
建模过程
问题描述
假设有三架无人机对移动目标进行协同定位,每架无人机上安装有被动雷达导引头,可提供高低角及方位角两种量测信息.请根据以下仿真条件,完成基于扩展卡尔曼滤波的协同定位滤波器设计及仿真分析.
仿真条件
-
初始时刻,地面系下目标状态位置初值:$p=[x, y, z]=[653.42,567.81,590.61] ~m$ ;速度初值: $v=[v_x,v_y,v_z]=[89.93,61.87,84.41] ~m/s$ ;加速度初值: $a=[a_x,a_y,a_z]~m/s^2$ ;目标状态初值的误差:位置误差 $\Delta p =[500,500,500]$ ;速度误差 $\Delta v = [50, 50, 50]$ ;加速度 $\Delta a = [5,5,5]$ (用于构造误差协方差矩阵初值).
-
目标运动模型选用"当前"统计模型,机动系数可选为 $\dfrac{1}{60}$;
-
导引头测角噪声为零均值高斯噪声,标准差 $\sigma = 0.5^{\circ}$;
-
导引头测量帧频为 $10\mathrm{HZ}$ ,共 $40\mathbf{s}$;
-
不考虑无人机姿态变化,并假设无人机本体系与地面系重合.
"当前"统计模型
以地理系为参考坐标系,选取地理系下目标运动的位置 $(x,y,z)$ ,速度 $(v_x,v_y,v_z)$ ,加速度 $(a_x,a_y,a_z)$ 为状态变量,有
根据"当前"统计模型建立的目标运动离散化方程为
其中,状态转移矩阵分别为
对于过程噪声协方差矩阵,为:
其中,
对于加速度方差 $\sigma$ 在每个轴上的分量如式表示
在本文中,假设 $a{max} = 15$, $a{-max} = -15$ .
系统量测方程
无人机量测数据为量测高低角 $\gamma$ 和量测方位角 $\eta$ .三架无人机即测量三架无人机以及量测高低角/方位角定义:
无人机与目标之间的相对距离需要转化至地面坐标系上,所以有:
所以,系统的量测方程为式:
其中 $[x~y~z]^T$ 为目标地面系下位置, $[x{s,t}, y{s,t}, z_{s,t}]]^T$ 为第以架无人机地面系的位置, $C_d^{s_i}$ 为地面系到第 $i$ 架无人机本体系的坐标转换矩阵,在仿真条件5下为 $C_d^{s_i}$ 单位阵.
仿真与分析
在MATLAB中基于以上仿真条件进行时长40s的系统仿真,具体结果如图1 - 4所示.
从图中可以看出,位置的收敛效果较好,在初始误差较大的情况下仅仅2秒后即可快速收敛;速度误差中x轴速度收敛效果较好,但是y轴和z轴尚未完全收敛,其中x轴和y轴误差在 $\pm 20 m/s$ 以内,x轴误差则在 $\pm 50 m/s$ 以内;加速度误差中收敛效果较差,其中x轴和y轴误差在 $\pm$ 10以内,z轴误差则在 $\pm$ 5以内.
可以看出,在给定40s的时间范围内,应用扩展卡尔曼滤波,对系统状态进行最优最优估计,可以对目标的位置进行较好的估计,但是对速度和加速度的估计较差.
⛳️ 运行结果
📣 部分代码
uav3 = load('./data/uav3.txt');target = load('./data/target.txt');measurement = load('./data/measurement.txt');measurement = measurement';%%T = 0.1;alpha = 1/60;I3 = eye(3);I9 = eye(9);O3= zeros(3);O31 = [0; 0; 0];N = 401;sigma = 0.5 * pi/180;R = sigma^2 * eye(6);a_max = 15;%%X0 = zeros(9, N);X0(:, 1) = [653.42, 567.81, 590.61, 89.93, 61.87, 84.7
🔗 参考文献
[1] 张寅孩.无速度传感器异步电机变频调速与位置伺服控制技术研究[D].浙江大学,2003.
[2] 杜荣华,张翔,王宁,等.非合作目标交会接近的自主检测和跟踪方法研究[J].宇航学报, 2021.DOI:10.3873/j.issn.1000-1328.2021.05.009.
[3] 詹东洲.适应环境和平台特征的水下移动节点声学定位研究[J].[2024-07-17].
🎈 部分理论引用网络文献，若有侵权联系博主删除
👇 关注我领取海量matlab电子书和数学建模资料
🎁 私信完整代码和数据获取及论文数模仿真定制🌈
🌈 各类智能优化算法改进及应用
生产调度、经济调度、装配线调度、充电优化、车间调度、发车优化、水库调度、三维装箱、物流选址、货位优化、公交排班优化、充电桩布局优化、车间布局优化、集装箱船配载优化、水泵组合优化、解医疗资源分配优化、设施布局优化、可视域基站和无人机选址优化、背包问题、 风电场布局、时隙分配优化、 最佳分布式发电单元分配、多阶段管道维修、 工厂-中心-需求点三级选址问题、 应急生活物质配送中心选址、 基站选址、 道路灯柱布置、 枢纽节点部署、 输电线路台风监测装置、 集装箱调度、 机组优化、 投资优化组合、云服务器组合优化、 天线线性阵列分布优化、CVRP问题、VRPPD问题、多中心VRP问题、多层网络的VRP问题、多中心多车型的VRP问题、 动态VRP问题、双层车辆路径规划（2E-VRP）、充电车辆路径规划（EVRP）、油电混合车辆路径规划、混合流水车间问题、 订单拆分调度问题、 公交车的调度排班优化问题、航班摆渡车辆调度问题、选址路径规划问题、港口调度
🌈 机器学习和深度学习时序、回归、分类、聚类和降维
2.1 bp时序、回归预测和分类
2.2 ENS声神经网络时序、回归预测和分类
2.3 SVM/CNN-SVM/LSSVM/RVM支持向量机系列时序、回归预测和分类
2.4 CNN/TCN卷积神经网络系列时序、回归预测和分类
2.5 ELM/KELM/RELM/DELM极限学习机系列时序、回归预测和分类
2.6 GRU/Bi-GRU/CNN-GRU/CNN-BiGRU门控神经网络时序、回归预测和分类
2.7 ELMAN递归神经网络时序、回归\预测和分类
2.8 LSTM/BiLSTM/CNN-LSTM/CNN-BiLSTM/长短记忆神经网络系列时序、回归预测和分类
2.9 RBF径向基神经网络时序、回归预测和分类
427
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
