协同导航MATLAB例程

程序介绍

单领航者的情况下，仅进行测距时，基于移动向量的协同导航/定位的MATLAB程序，带中文注释。
领航者有良好的定位，跟随者只测量与领航者之间的相对距离

程序源码

% 单领航者 仅测距，基于移动向量(Moving Radius Vector,MRV)的协同定位
% author:Evand
% 作者联系方式：evandjiang@qq.com（除前期达成一致外，付费咨询）
% date: 2024-3-4
% Ver2，添加轨迹
clear;clc;close all;
T = 0.1; %采样率
t = [T:T:100]';
rng(0);
%% initial
Lx = 0.5*t; %领航者 leader
Ly = 1*t;
Fx_i = -10+0.1*cos(t/20+pi/180*20)+0.7*t; %跟随者 follower
Fy_i = 15+0.4*sin(t/10+pi/180*41)+0.9*t;
% V_Fx = -1*sin(t+pi/180*20)+0.001;
% V_Fy = 2*0.4*cos(2*t+pi/180*41)+0.01;
V_Fx_i = [diff(Fx_i);0]/T;
V_Fy_i = [diff(Fy_i);0]/T;
V_F_i = (V_Fx_i.^2+V_Fy_i.^2).^0.5;
phi_i = atand(V_Fy_i./V_Fx_i); %【有奇异点】
Q = 0.1*diag([1,1,1]);w=sqrt(Q)*randn(size(Q,1),length(t)); 
R = 1*diag([1,1]);v=sqrt(R)*randn(size(R,1),length(t));
P =0.1*eye(3);
V_Fx = V_Fx_i + w(1,:)'+ones(length(t),1).*[0.01:0.01:length(t)*0.01]'*T; %【】
V_Fy = V_Fy_i + w(2,:)'+ones(length(t),1).*[0.01:0.01:length(t)*0.01]'*T;
V_F = (V_Fx.^2+V_Fy.^2).^0.5;
omega = [diff(phi_i);0]/T+w(3,:)';%+ones(length(t),1).*[0.01:0.01:length(t)*0.01]'*T;

Fx = [0;cumsum(V_Fx(1:end-1))]*T+Fx_i(1);
Fy = [0;cumsum(V_Fy(1:end-1))]*T+Fy_i(1);
phi = [0;cumsum(omega(1:end-1))]*T+phi_i(1);
x = [Fx,Fy,phi];
diss = ((Lx-Fx_i).^2+(Ly-Fy_i).^2).^0.5; %领航者与跟随者之间的距离

结果图

轨迹图：

滤波误差图（三轴时序）：

完整源码下载链接

https://download.csdn.net/download/callmeup/89128668