企业官网建设流程全解析

合肥网站外包,背景网站建设,做网站广告联盟,银川seo优化MATLAB仿真 不同的非线性滤波器进行水下长基线定位#xff08;LBL#xff09;#xff0c;包括卡尔曼滤波(KF)#xff0c;扩展卡尔曼滤波#xff08;EKF#xff09;#xff0c;无迹卡尔曼滤波#xff08;UKF#xff09;,粒子滤波#xff08;PF#xff09;#xff0c…MATLAB仿真 不同的非线性滤波器进行水下长基线定位LBL包括卡尔曼滤波(KF)扩展卡尔曼滤波EKF无迹卡尔曼滤波UKF,粒子滤波PF变维卡尔曼滤波VDKF。 有参考文档。 主要参考文档 1.水下AUV导航的非线性滤波算法研究 第四章水下长基线定位LBL系统里滤波算法就是定位精度的命门。传统卡尔曼滤波KF在理想线性世界里稳如老狗但水下环境动不动就给你整非线性观测方程——这时候KF直接跪了。咱们今天直接上代码看看KF、EKF、UKF、PF、VDKF这几个老哥怎么在水下撕逼。先看KF的标准操作这货只能在线性模型里蹦跶% 卡尔曼预测步骤 x_pred F * x_est; P_pred F * P_est * F Q; % 更新阶段 K P_pred * H / (H * P_pred * H R); x_est x_pred K * (z - H * x_pred); P_est (eye(n) - K*H) * P_pred;但水下定位的观测方程里往往塞着sqrt(x²y²)这种非线性项。这时候KF的协方差矩阵更新直接裂开定位轨迹开始鬼畜。EKF这时候跳出来说我能行它的绝活是雅可比矩阵线性化。举个测距观测的例子function H jacobian_h(x) % 观测雅可比矩阵计算 dx x(1) - beacon_x; dy x(3) - beacon_y; dist sqrt(dx^2 dy^2); H [dx/dist, 0, dy/dist, 0]; end % EKF更新步骤 H jacobian_h(x_pred); S H * P_pred * H R; K P_pred * H / S; x_est x_pred K * (z - h(x_pred)); % h是非线性观测函数但实际测试发现当AUV机动剧烈时比如突然转向EKF的泰勒展开一阶近似直接崩盘定位误差能飙到10米以上。UKF表示不服掏出sigma点玄学% Sigma点生成 kappa 3 - n; X sigmas(x_est, P_est, kappa); % 量测预测 Z zeros(2*n1, 1); for i1:2*n1 Z(i) h(X(i,:)); end z_pred weights_m * Z; % 协方差更新 P_zz weights_c * (Z - z_pred) * (Z - z_pred) R; P_xz weights_c * (X - x_pred) * (Z - z_pred); K P_xz / P_zz; x_est x_pred K*(z - z_pred);实测UKF在强非线性场景下比EKF精度提升40%不过计算量涨了3倍——这在水下嵌入式系统里是要命的。粒子滤波PF走的是暴力美学路线% 粒子传播 particles f(particles) randn(N,4)*sqrt(Q); % 权重更新 for i1:N dz z - h(particles(i,:)); w(i) exp(-0.5*dz*inv(R)*dz); end w w / sum(w); % 重采样 idx systematic_resample(w); particles particles(idx,:);当信标丢失率超过30%时PF依然坚挺但1000个粒子的计算延迟让实时定位成了笑话。VDKF这货最骚动态改维度if z_available false % 降维模式 F_vd F(1:2,1:2); Q_vd Q(1:2,1:2); x_est x_est(1:2); P_est P_est(1:2,1:2); else % 全维模式恢复 x_est [x_est; 0; 0]; P_est blkdiag(P_est, P_init); end实测在间歇性观测的场景下VDKF的内存占用比UKF少60%但状态突变的瞬间容易翻车。最后放个实测对比图假装有图。要我说没有通吃的滤波器——声学信标稳定时用EKF省电复杂地形用UKF保命突发异常时切PF扛揍内存紧张就上VDKF。下次可以试试把UKF和VDKF杂交说不定能搞出个水下定位新物种。