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

💥💥💞💞欢迎来到本博客❤️❤️💥💥

🏆博主优势：🌞🌞🌞博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。

⛳️座右铭：行百里者，半于九十。

📋📋📋本文目录如下：🎁🎁🎁

目录

💥1 概述

📚2 运行结果

🎉3 参考文献

🌈4 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代码实现