【信号处理】扩展卡尔曼滤波EKF(Matlab代码实现)

简介: 【信号处理】扩展卡尔曼滤波EKF(Matlab代码实现)

💥1 概述

扩展卡尔曼滤波(extended kalman filter,EKF)是在线性最小方差估计的基础上,提出的一种实时递推型的最优估计滤波算法,近年来被逐步应用于工程结构的参数识别研究[1.2.3.4.5]当中。为解决传统EKF算法中其状态向量维度过大导致该算法运行效率变慢、识别精度下降等问题,本文提出了一种改进的EKF算法,引入模态坐标变换,利用模态坐标转换对传统EKF的状态向量进行处理,构建以模态坐标初始值和结构损伤参数的状态向量。由于结构振动响应通常由前几阶的低阶频率成分组成,因此,对复杂结构可以有效缩减状态向量的维数,以保证算法的稳定性及准确性。


📚2 运行结果

部分代码:

%观测站的位置
x1=-10;
y1=0;
x2=10;
y2=0;
r1 = sqrt((Rx-x1)^2+(Ry-y1)^2);
beta1 = atan2((Ry-y1),(Rx-x1));
r2 = sqrt((Rx-x2)^2+(Ry-y2)^2);
beta2 = atan2((Ry-y2),(Rx-x2));
%noise
sigma_u = sqrt(0.0001);     %过程噪声
sigma_R = sqrt(5);        %距离量测噪声
sigma_beta = sqrt(0.0001);    %角度量测噪声
%% Kalman filter CV 2D
%-------Kalman Parameters-------%
A1 = [cos(beta1) -r1*sin(beta1); sin(beta1) r1*cos(beta1)] ;
R1 = A1*[sigma_R^2 0;0 sigma_beta^2]*A1' ;
P1 = [R1(1,1)   R1(1,1)/T     R1(1,2)   R1(1,2)/T
    R1(1,1)/T 2*R1(1,1)/T^2 R1(1,2)/T 2*R1(1,2)/T^2
    R1(1,2)   R1(1,2)/T     R1(2,2)   R1(2,2)/T
    R1(1,2)/T 2*R1(1,2)/T^2 R1(2,2)/T 2*R1(2,2)/T^2 ];
% P1 = 100*eye(4);
A2 = [cos(beta2) -r2*sin(beta2); sin(beta2) r2*cos(beta2)] ;
R2 = A2*[sigma_R^2 0;0 sigma_beta^2]*A2' ;
P2 = [R2(1,1)   R2(1,1)/T     R2(1,2)   R2(1,2)/T
    R2(1,1)/T 2*R2(1,1)/T^2 R2(1,2)/T 2*R2(1,2)/T^2
    R2(1,2)   R2(1,2)/T     R2(2,2)   R2(2,2)/T
    R2(1,2)/T 2*R2(1,2)/T^2 R2(2,2)/T 2*R2(2,2)/T^2 ];
% P2 = 100*eye(4);
%状态转移矩阵
F = [1 T 0 0 
     0 1 0 0
     0 0 1 T
     0 0 0 1];
%过程噪声
B = [T^2/2, 0; T, 0;
     0, T^2/2; 0, T]; %过程噪声分布矩阵
v = sigma_u^2;   %x方向的过程噪声向量//相当于Q
V = B * v * B';
% %观测噪声??
% W = B * noise_x;
%------Data initial-------%
X_real = zeros(4,N);
X = zeros(4,N);
Z1 = zeros(2,N);
Z_polar1 = zeros(2,N);
X_EKF1 = zeros(4,N);
Z2 = zeros(2,N);
Z_polar2 = zeros(2,N);
X_EKF2 = zeros(4,N);
X_CC = zeros(4,N);
X_BC = zeros(4,N);
bias = zeros(8,N,M);
%-------Track Initial-------%
X_real(:,1) = [Rx, vx, Ry, vy]'; %x: km,km/s
X_EKF1(:,1) = [Rx, 0, Ry, 0];
X_EKF2(:,1) = [Rx, 0, Ry, 0];
X_CC(:,1) = [Rx, 0, Ry, 0];
X_BC(:,1) = [Rx, 0, Ry, 0];
%Monto-carlo
for m=1:M
    noise_u = randn(2,N).*sigma_u;
    noise_R = randn(1,N).*sigma_R; %过程噪声
    noise_beta = randn(1,N).*sigma_beta; %观测噪声


🎉3 参考文献

部分理论来源于网络,如有侵权请联系删除。


[1]刘志勇,王阿利,王小红.一种基于自构架模糊EKF的目标跟踪方法[J].计算技术与自动化,2022,41(03):14-20.DOI:10.16339/j.cnki.jsjsyzdh.202203003.


[2]胡文强,胡建鹏,吴飞,陆雯霞.基于EKF的初始状态自适应室内融合定位算法[J].传感器与微系统,2022,41(11):147-151.DOI:10.13873/J.1000-9787(2022)11-0147-05.


🌈4 Matlab代码实现

相关文章
|
12天前
|
传感器 资源调度 算法
基于无迹卡尔曼滤波(UKF)与模型预测控制(MPC)的多无人机避撞研究(Matlab代码实现)
基于无迹卡尔曼滤波(UKF)与模型预测控制(MPC)的多无人机避撞研究(Matlab代码实现)
|
14天前
|
机器学习/深度学习 传感器 算法
基于不变扩展卡尔曼滤波器RI-EKF的同时定位与地图构建SLAM算法的收敛性和一致性特性研究(Matlab代码实现)
基于不变扩展卡尔曼滤波器RI-EKF的同时定位与地图构建SLAM算法的收敛性和一致性特性研究(Matlab代码实现)
|
17天前
|
传感器 算法 Shell
【使用卡尔曼滤波器将陀螺仪和加速度计的读数融合,以获取IMU的姿态(四元数)】实现了所谓的“零速度更新”算法,用于行人跟踪(步态跟踪)(Matlab代码实现)
【使用卡尔曼滤波器将陀螺仪和加速度计的读数融合,以获取IMU的姿态(四元数)】实现了所谓的“零速度更新”算法,用于行人跟踪(步态跟踪)(Matlab代码实现)
|
23天前
|
传感器 算法 自动驾驶
【卡尔曼滤波跟踪】基于卡尔曼滤波的二维目标跟踪(Matlab实现)
【卡尔曼滤波跟踪】基于卡尔曼滤波的二维目标跟踪(Matlab实现)
109 0
|
10天前
|
算法 计算机视觉
【MPDR & SMI】失配广义夹角随输入信噪比变化趋势、输出信干噪比随输入信噪比变化趋势研究(Matlab代码实现)
【MPDR & SMI】失配广义夹角随输入信噪比变化趋势、输出信干噪比随输入信噪比变化趋势研究(Matlab代码实现)
|
10天前
|
编解码 人工智能 算法
【采用BPSK或GMSK的Turbo码】MSK、GMSK调制二比特差分解调、turbo+BPSK、turbo+GMSK研究(Matlab代码实现)
【采用BPSK或GMSK的Turbo码】MSK、GMSK调制二比特差分解调、turbo+BPSK、turbo+GMSK研究(Matlab代码实现)
|
10天前
|
机器学习/深度学习 编解码 并行计算
【改进引导滤波器】各向异性引导滤波器,利用加权平均来实现最大扩散,同时保持图像中的强边缘,实现强各向异性滤波,同时保持原始引导滤波器的低低计算成本(Matlab代码实现)
【改进引导滤波器】各向异性引导滤波器,利用加权平均来实现最大扩散,同时保持图像中的强边缘,实现强各向异性滤波,同时保持原始引导滤波器的低低计算成本(Matlab代码实现)
|
9天前
|
存储 编解码 算法
【多光谱滤波器阵列设计的最优球体填充】使用MSFA设计方法进行各种重建算法时,图像质量可以提高至多2 dB,并在光谱相似性方面实现了显著提升(Matlab代码实现)
【多光谱滤波器阵列设计的最优球体填充】使用MSFA设计方法进行各种重建算法时,图像质量可以提高至多2 dB,并在光谱相似性方面实现了显著提升(Matlab代码实现)
|
10天前
|
传感器 机器学习/深度学习 算法
【UASNs、AUV】无人机自主水下传感网络中遗传算法的路径规划问题研究(Matlab代码实现)
【UASNs、AUV】无人机自主水下传感网络中遗传算法的路径规划问题研究(Matlab代码实现)
|
9天前
|
机器学习/深度学习 传感器 算法
【高创新】基于优化的自适应差分导纳算法的改进最大功率点跟踪研究(Matlab代码实现)
【高创新】基于优化的自适应差分导纳算法的改进最大功率点跟踪研究(Matlab代码实现)
91 14

热门文章

最新文章