USBL与IMU融合的导航程序，MATLAB编写的仿真例程，（超短基线与惯导结合，用扩展卡尔曼滤波做融合）附完整代码

USBL（Ultra Short BaseLine，超短基线）与IMU（Inertial Measurement Unit，惯性测量原件）融合的导航MATLAB代码，真值、USBL观测、IMU推算、EKF融合后的结果，多个对比。给出图片和误差特性的数字分析
订阅专栏后，可直接查看源代码，粘贴到MATLAB空脚本中即可直接运行、得到结果

运行结果

轨迹图：

三轴位移曲线：

误差对比曲线：

命令行窗口的截图：

MATLAB源代码

完整代码如下：

% USBL（超短基线）定位系统仿真，三维，基于方位角与距离测量的目标定位仿真（EKF实现）
% 观测模型：方位角θ：基于目标与传感器的XY平面投影计算,俯仰角φ：基于高度差和目标水平距离计算,斜距r：欧氏距离