💥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.