Visual Homing Project: Homing algorithm simulation and convergence test.

As a part of the homing project, I’ve been working on a paper by Liu et. al. concerning a bearing only method for visual homing. I have now implemented everything on ROS and am now in the testing stages for the implementation on a quadcopter. But before all of that, I had to test for convergence of the algorithm, because the paper is not the most lucid one you’d find.

So I have written a small piece of code on MATLAB that when run, produces the proof of convergence of the algorithm.

Here are the code(s):

  • Algo.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
clc;
clear;
close all;
 
global u;
 
 
Tsamp = 1;
 
 
X0 = [60 30]';
 
G = [12 -42]';
u = [0 0]';
 
L1 = [150 30]';
L2 = [-100 -180]';
L3 = [-170 -60]';
 
theta_1 = newTan(L1(2)- G(2), L1(1)-G(1));
theta_2 = newTan(L2(2)-G(2), L2(1)-G(1));
theta_3 = newTan(L3(2)-G(2), L3(1)-G(1));
 
beta_21 = theta_2 - theta_1;
beta_21s = newTan(sin(beta_21),cos(beta_21));
 
beta_32 = theta_3 - theta_2;
beta_32s = newTan(sin(beta_32),cos(beta_32));
 
beta_13 = theta_1 - theta_3;
beta_13s = newTan(sin(beta_13),cos(beta_13));
 
path = zeros(2,1);
path(:,1) = X0;
 
for k=1:1000
    [t,X] = ode45('holModel',Tsamp, path(:,end));
    theta_1 = newTan(L1(2)- X(end,2), L1(1)- X(end,1));
    theta_2 = newTan(L2(2)- X(end,2), L2(1)- X(end,1));
    theta_3 = newTan(L3(2)- X(end,2), L3(1)- X(end,1));
 
    beta_21 = theta_2 - theta_1;
    beta_21 = newTan(sin(beta_21),cos(beta_21));
 
    beta_32 = theta_3 - theta_2;
    beta_32 = newTan(sin(beta_32),cos(beta_32));
 
    beta_13 = theta_1 - theta_3;
    beta_13 = newTan(sin(beta_13),cos(beta_13));
 
    OP1 = L1 - X(end,:)';
    OP1 = OP1./norm(OP1);
    OP2 = L2 - X(end,:)';
    OP2 = OP2./norm(OP2);
    OP3 = L3 - X(end,:)';
    OP3 = OP3./norm(OP3);
 
    M = [cos(beta_13/2)*(beta_13 - beta_13s) 0 cos(beta_13/2)*(beta_13 - beta_13s);
        cos(beta_21/2)*(beta_21 - beta_21s) cos(beta_21/2)*(beta_21 - beta_21s) 0;
        0 cos(beta_32/2)*(beta_32 - beta_32s) cos(beta_32/2)*(beta_32 - beta_32s)];
    N = [OP1 OP2 OP3]';
 
    V = 2.*M*N;
 
    u = -(V(1,:) + V(2,:) +V(3,:))';
 
    path = [path X(end,:)'];
 
    if(norm(path(:,end) - G)<.1)
        break;
    end
 
end
 
figure(1); plot(path(1,:),path(2,:)); hold on;
  • newTan.m
1
2
3
4
5
6
7
function [ phi ] = newTan( sinTheta, cosTheta )
%UNTITLED Summary of this function goes here
%   Detailed explanation goes here
 
 phi = atan2(sinTheta, cosTheta) + (atan2(sinTheta, cosTheta) < 0)*2*pi;
 
end
  • holModel.m
1
2
3
4
5
6
7
function dydt = holModel(t,X0)
 
global u;
 
dydt =  u;
 
end

Which upon running produce the proof of convergence of the algorithm. Such as this, where start point is (60,30) and end point is (-12,-42)

convergence

 

One thought on “Visual Homing Project: Homing algorithm simulation and convergence test.

Leave a Reply

Your email address will not be published. Required fields are marked *