协同导航的MATLAB程序，采用EKF作为滤波算法

该代码实现了基于移动向量（Moving Radius Vector, MRV）的单领航者协同导航系统，采用扩展卡尔曼滤波（EKF）算法对跟随者（Follower）的位置和航向角进行估计。系统通过领航者（Leader）的已知轨迹和跟随者的测距信息，结合惯性导航数据，实现对跟随者的高精度定位

算法核心原理

• 协同定位框架
  领航者具有精确的定位信息（通过GNSS/INS），而跟随者仅能测量与领航者的距离。代码中通过领航者的线性运动轨迹（Lx=0.5*t, Ly=1*t）和跟随者的非线性运动轨迹（Fx_i和Fy_i）模拟实际场景

• 运动模型
  跟随者的运动状态通过速度（V_Fx, V_Fy）和角速度（omega）描述，状态方程基于航位推算（Dead Reckoning）更新，包含过程噪声（协方差矩阵Q）

• 观测模型
  测距观测值diss模拟跟随者与领航者之间的实际距离，并加入高斯噪声（协方差矩阵