Skip to content

Available Functions and Documentation

Apart from the algorithms, several auxiliary functions are available. In this page, one can find all the available functions and their respective documentation.

Algorithms

gtrs 

gtrs(
    a_i: ArrayLike,
    n: int,
    k: int,
    destinations: ArrayLike,
    initial_uav_position: ArrayLike,
    v_max: float,
    tau: float,
    gamma: float,
    noise_seed: int = 1,
    noise_distribution: str = "standard_normal",
    distribution_parameters: Optional[ArrayLike] = None,
    tol: float = 0.001,
    n_iter: int = 30,
    max_lim: float = 1000000.0,
) -> NDArray

Executes the GTRS algorithm.

See here more details about the GTRS algorithm.

Parameters:

Name Type Description Default
a_i ArrayLike

The true position of the anchors in 3D.

 required
n int

The number of anchors.

 required
k int

The number of measurements.

 required
destinations ArrayLike

The intermediate points need for navigation in 3D.

 required
initial_uav_position ArrayLike

The initial UAV position in 3D.

 required
v_max float

The maximum velocity that the UAV can fly.

 required
tau float

The threshold to reach the destination.

 required
gamma float

The smoothing factor.

 required
noise_seed int

The seed to generate the noise.

 1
noise_distribution str

The distribution used to model the noise.

 'standard_normal'
distribution_parameters Optional[ArrayLike]

Optional parameters to customize the noise distribution.

 None
tol float

The tolerance for the bisection function.

 0.001
n_iter int

The max number of iterations for the bisection function.

 30
max_lim float

The maximum value for the interval in the bisection function.

 1000000.0

Returns:

Type Description
NDArray

The estimated positions for the UAV computed using the GTRS algorithm for the given input scenario
NDArray

and the true trajectory that the UAV followed.
Source code in autonav/GTRS.py 
 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
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
def gtrs(
    a_i: ArrayLike,
    n: int,
    k: int,
    destinations: ArrayLike,
    initial_uav_position: ArrayLike,
    v_max: float,
    tau: float,
    gamma: float,
    noise_seed: int = 1,
    noise_distribution: str = "standard_normal",
    distribution_parameters: Optional[ArrayLike] = None,
    tol: float = 0.001,
    n_iter: int = 30,
    max_lim: float = 1000000.0,
) -> NDArray:
    """Executes the GTRS algorithm.

    [See here more details about the GTRS algorithm.](https://ieeexplore.ieee.org/document/9456863)

    Args:
        a_i: The true position of the anchors in 3D.
        n: The number of anchors.
        k: The number of measurements.
        destinations: The intermediate points need for navigation in 3D.
        initial_uav_position: The initial UAV position in 3D.
        v_max: The maximum velocity that the UAV can fly.
        tau: The threshold to reach the destination.
        gamma: The smoothing factor.
        noise_seed: The seed to generate the noise.
        noise_distribution: The distribution used to model the noise.
        distribution_parameters: Optional parameters to customize the noise distribution.
        tol: The tolerance for the bisection function.
        n_iter: The max number of iterations for the bisection function.
        max_lim: The maximum value for the interval in the bisection function.

    Returns:
        The estimated positions for the UAV computed using the GTRS algorithm for the given input scenario
        and the true trajectory that the UAV followed.
    """
    # Transform inputs in NDArray
    arr_a_i: NDArray = asarray(a_i, dtype=float)
    arr_destinations: NDArray = asarray(destinations, dtype=float)
    arr_initial_uav_position: NDArray = asarray(initial_uav_position, dtype=float)
    if distribution_parameters is None:
        arr_distribution_parameters: NDArray = asarray([])
    else:
        arr_distribution_parameters = asarray(distribution_parameters, dtype=float)
    # Validate inputs
    if size(arr_a_i, axis=1) != n:
        raise ValueError("The length of a_i must be equal to N.")
    if k <= 0:
        raise ValueError("K must be positive.")
    if size(arr_destinations) == 0:
        raise ValueError("Waypoints cannot be empty.")
    if size(arr_destinations, axis=1) != 3:
        raise ValueError("Waypoints must contain the 3 coordinates (x, y, z).")
    if size(arr_initial_uav_position) != 3:
        raise ValueError("Initial UAV position must contain the 3 coordinates (x, y, z).")
    # Test optional inputs
    available_noise_distributions = ["normal", "standard_normal", "exponential"]
    if noise_distribution not in available_noise_distributions:
        raise ValueError("Noise distribution must be " + ", ".join(available_noise_distributions) + ".")
    if tol < 0:
        raise ValueError("Tolerance must be positive.")
    if n_iter < 0:
        raise ValueError("Number of Bisection iterations must be positive.")
    if max_lim < 0:
        raise ValueError("The maximum value for the interval in the bisection function must be positive.")
    # Time sample in seconds
    ts = 1
    # State transition matrix
    s = eye(6)
    s[0, 3] = ts
    s[1, 4] = ts
    s[2, 5] = ts
    # State process noise intensity
    sigma_w = 0.05
    # State process noise covariance
    q = dot(
        sigma_w**2,
        (
            [
                [ts**3 / 3, 0, 0, ts**2 / 2, 0, 0],
                [0, ts**3 / 3, 0, 0, ts**2 / 2, 0],
                [0, 0, ts**3 / 3, 0, 0, ts**2 / 2],
                [ts**2 / 2, 0, 0, ts, 0, 0],
                [0, ts**2 / 2, 0, 0, ts, 0],
                [0, 0, ts**2 / 2, 0, 0, ts],
            ]
        ),
    )
    x_state = zeros(shape=(6, 1))
    x_loc = zeros(shape=(3, 1))
    p = eye(6)
    qq = 0
    x_true = arr_initial_uav_position[:]
    estimated_positions = []
    true_trajectory = []
    ww = 0
    n_dest = len(arr_destinations) - 1
    # Generator to create random numbers
    gen = random_generator(noise_seed)
    while ww <= n_dest:
        distance = math.sqrt(
            (x_true[0] - arr_destinations[ww][0]) ** 2
            + (x_true[1] - arr_destinations[ww][1]) ** 2
            + (x_true[2] - arr_destinations[ww][2]) ** 2
        )
        while distance > 1:
            x = x_true[:]
            # ---------------------------------------------------------------------
            # Simulation part
            # ---------------------------------------------------------------------
            di_k = sqrt(((x[0] - arr_a_i[0, :]) ** 2) + ((x[1] - arr_a_i[1, :]) ** 2) + ((x[2] - arr_a_i[2, :]) ** 2))
            di_k = array([di_k]).T
            if noise_distribution == "normal":
                # See if user passed the mean and std
                if size(arr_distribution_parameters) == 2:
                    mean = arr_distribution_parameters[0]
                    std = arr_distribution_parameters[1]
                    noise_model = gen.normal(loc=mean, scale=std, size=(n, k))
                else:
                    noise_model = gen.normal(size=(n, k))
                di_k = di_k + noise_model
            elif noise_distribution == "exponential":
                # See if user passed the rate
                if size(arr_distribution_parameters) == 1:
                    rate = arr_distribution_parameters[0]
                    noise_model = gen.exponential(scale=rate, size=(n, k))
                else:
                    rate = 10**-6
                    noise_model = gen.exponential(scale=rate, size=(n, k))
                di_k = di_k + noise_model
            elif noise_distribution == "standard_normal":  # Default distribution
                noise_model = gen.standard_normal(size=(n, k))
                di_k = di_k + noise_model
            d_i = median(di_k, axis=1)
            d_i = array([d_i]).T
            # ---------------------------------------------------------------------
            # Estimation part
            # ---------------------------------------------------------------------
            a1_loc = zeros(shape=(n, 4))
            b1_loc = zeros(shape=(n, 1))
            a_track = zeros(shape=(n + 6, 7))
            b_track = zeros(shape=(n + 6, 1))
            w_i_loc = array(sqrt(d_i ** (-1.0) / (sum(d_i ** (-1.0)))))
            for tt in arange(0, n, 1).reshape(-1):
                a1_loc[tt] = append(dot(2, arr_a_i[0:3, tt].T), -1)
                b1_loc[tt] = norm(arr_a_i[0:3, tt]) ** 2 - d_i[tt] ** 2
            w_loc = diag(w_i_loc.T[0])
            d_loc = eye(4)
            d_loc[3, 3] = 0
            f_loc = array([0, 0, 0, -1.0 / 2.0]).reshape(4, 1)
            a_loc = dot(w_loc, a1_loc)
            b_loc = dot(w_loc, b1_loc)
            if qq != 0:
                aux = dot(s, p)
                p_pred = dot(aux, s.T) + q
                x_pred = dot(s, x_state[0:6, qq - 1])
                x_pred = x_pred.reshape(len(x_pred), 1)
                a1_track = zeros(shape=(n, 7))
                b1_track = zeros(shape=(n, 1))
                for tt in arange(0, n, 1).reshape(-1):
                    a1_track[tt] = concatenate((dot(2, arr_a_i[0:3, tt].T), zeros(size(x, 0)), [-1]), axis=0)
                    b1_track[tt] = norm(arr_a_i[0:3, tt]) ** 2 - abs(d_i[tt] ** 2)
                left_matrix = sqrtm(inv(p_pred))
                right_matrix = zeros((size(x_state, 0), 1))
                a1_track = concatenate((a1_track, concatenate((left_matrix, right_matrix), axis=1)), axis=0)
                a1_track[a1_track == math.inf] = 0
                inf_p_pred = array(sqrtm(inv(p_pred)))
                inf_p_pred[inf_p_pred == math.inf] = 0
                b1_track = concatenate((b1_track, dot(inf_p_pred, x_pred)), axis=0)
                a = dot(math.sqrt(1.0 / 2.0), w_i_loc.T)
                b = dot(math.sqrt(1.0 / 8.0), ones((1, size(x_state, 0))))
                w_track = concatenate((a, b), axis=1)
                w_track = eye(size(w_track, 1)) * w_track
                d_track = zeros((7, 7))
                d_track[0, 0] = 1
                d_track[1, 1] = 1
                d_track[2, 2] = 1
                f_track = array([0, 0, 0, 0, 0, 0, -1.0 / 2.0]).reshape(7, 1)
                a_track = dot(w_track, a1_track)
                b_track = dot(w_track, b1_track)
            eigen_values = _calc_eigen(a_loc, d_loc)
            eig_1 = max(eigen_values)
            min_lim = -1.0 / eig_1
            lambda_loc = _bisection_fun(min_lim, max_lim, tol, n_iter, a_loc, d_loc, b_loc, f_loc)
            y_hat_loc = solve(
                (dot(a_loc.T, a_loc) + dot(lambda_loc, d_loc) + dot(1e-06, eye(size(a_loc, 1)))),
                (dot(a_loc.T, b_loc) - dot(lambda_loc, f_loc)),
            )
            if qq == 0:
                x_loc[0:3, qq] = real(y_hat_loc[0:3, 0])
                x_state[0:6, qq] = concatenate((x_loc[0:3, qq], zeros(3)), axis=0)
                p = eye(6)
                estimated_positions.append(x_loc[0:3, qq])
            else:
                x_loc = insert(x_loc, qq, real(y_hat_loc[0:3, 0]), axis=1)
                eigen_values = _calc_eigen(a_track, d_track)
                eig_1 = max(eigen_values)
                min_lim = -1.0 / eig_1
                lambda_track = _bisection_fun(min_lim, max_lim, tol, n_iter, a_track, d_track, b_track, f_track)
                y_hat_track = solve(
                    (dot(a_track.T, a_track) + dot(lambda_track, d_track) + dot(1e-06, eye(size(a_track, 1)))),
                    (dot(a_track.T, b_track) - dot(lambda_track, f_track)),
                )
                x_state = concatenate((x_state, zeros((size(x_state, 0), 1))), axis=1)
                x_state[0:6, qq] = concatenate((real(y_hat_track[arange(0, size(x_state, 0))])), axis=0)
                lk1 = subtract(x_state[0:6, qq], x_state[0:6, qq - 1]).reshape((6, 1))
                lk2 = subtract(x_state[0:6, qq], x_state[0:6, qq - 1]).reshape((6, 1)).T
                p = matmul(lk1, lk2)
                estimated_positions.append(x_loc[0:3, qq])
            true_trajectory.append(x_true.copy())
            uav_velocity = _velocity(x_loc[0:3, qq], arr_destinations[ww, :], v_max, tau, gamma)
            x_true[0] = x_true[0] + uav_velocity[0]
            x_true[1] = x_true[1] + uav_velocity[1]
            x_true[2] = x_true[2] + uav_velocity[2]
            distance = math.sqrt(
                (x_true[0] - arr_destinations[ww][0]) ** 2
                + (x_true[1] - arr_destinations[ww][1]) ** 2
                + (x_true[2] - arr_destinations[ww][2]) ** 2
            )
            qq += 1
        ww += 1
    return array([array(estimated_positions), array(true_trajectory)])

wls 

wls(
    a_i: ArrayLike,
    n: int,
    k: int,
    destinations: ArrayLike,
    initial_uav_position: ArrayLike,
    v_max: float,
    tau: float,
    gamma: float,
    noise_seed: int = 1,
    noise_distribution: str = "standard_normal",
    distribution_parameters: Optional[ArrayLike] = None,
) -> NDArray

Executes the WLS algorithm.

[See here more details about the WLS algorithm.] (https://ietresearch.onlinelibrary.wiley.com/doi/full/10.1049/wss2.12041)

Parameters:

Name Type Description Default
a_i ArrayLike

The true position of the anchors in 3D.

 required
n int

The number of anchors.

 required
k int

The number of measurements.

 required
destinations ArrayLike

The intermediate points need for navigation in 3D.

 required
initial_uav_position ArrayLike

The initial UAV position in 3D.

 required
v_max float

The maximum velocity that the UAV can fly.

 required
tau float

The threshold to reach the destination.

 required
gamma float

The smoothing factor.

 required
noise_seed int

The seed to generate the noise.

 1
noise_distribution str

The distribution used to model the noise.

 'standard_normal'
distribution_parameters Optional[ArrayLike]

Optional parameters to customize the noise distribution.

 None

Returns:

Type Description
NDArray

The estimated positions for the UAV computed using the WLS algorithm for the given input scenario
NDArray

and the true trajectory that the UAV followed.
Source code in autonav/WLS.py 
 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
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
def wls(
    a_i: ArrayLike,
    n: int,
    k: int,
    destinations: ArrayLike,
    initial_uav_position: ArrayLike,
    v_max: float,
    tau: float,
    gamma: float,
    noise_seed: int = 1,
    noise_distribution: str = "standard_normal",
    distribution_parameters: Optional[ArrayLike] = None,
) -> NDArray:
    """Executes the WLS algorithm.

    [See here more details about the WLS algorithm.]
    (https://ietresearch.onlinelibrary.wiley.com/doi/full/10.1049/wss2.12041)

    Args:
        a_i: The true position of the anchors in 3D.
        n: The number of anchors.
        k: The number of measurements.
        destinations: The intermediate points need for navigation in 3D.
        initial_uav_position: The initial UAV position in 3D.
        v_max: The maximum velocity that the UAV can fly.
        tau: The threshold to reach the destination.
        gamma: The smoothing factor.
        noise_seed: The seed to generate the noise.
        noise_distribution: The distribution used to model the noise.
        distribution_parameters: Optional parameters to customize the noise distribution.

    Returns:
        The estimated positions for the UAV computed using the WLS algorithm for the given input scenario
        and the true trajectory that the UAV followed.
    """
    # Transform inputs in NDArray
    arr_a_i: NDArray = asarray(a_i, dtype=float)
    arr_destinations: NDArray = asarray(destinations, dtype=float)
    arr_initial_uav_position: NDArray = asarray(initial_uav_position, dtype=float)
    if distribution_parameters is None:
        arr_distribution_parameters: NDArray = asarray([])
    else:
        arr_distribution_parameters = asarray(distribution_parameters, dtype=float)
    # Validate inputs
    if size(arr_a_i, axis=1) != n:
        raise ValueError("The length of a_i must be equal to N.")
    if k <= 0:
        raise ValueError("K must be positive.")
    if size(arr_destinations) == 0:
        raise ValueError("Waypoints cannot be empty.")
    if size(arr_destinations, axis=1) != 3:
        raise ValueError("Waypoints must contain the 3 coordinates (x, y, z).")
    if size(initial_uav_position) != 3:
        raise ValueError("Initial UAV position must contain the 3 coordinates (x, y, z).")
    # Test optional inputs
    available_noise_distributions = ["normal", "standard_normal", "exponential"]
    if noise_distribution not in available_noise_distributions:
        raise ValueError("Noise distribution must be " + ", ".join(available_noise_distributions) + ".")
    x_true = arr_initial_uav_position[:]
    ww = 0
    n_dest = len(arr_destinations) - 1
    estimated_positions = []
    true_trajectory = []
    # Generator to create random numbers (see line 65)
    gen = random_generator(noise_seed)
    while ww <= n_dest:
        distance = math.sqrt(
            (x_true[0] - arr_destinations[ww][0]) ** 2
            + (x_true[1] - arr_destinations[ww][1]) ** 2
            + (x_true[2] - arr_destinations[ww][2]) ** 2
        )
        while distance > 1:
            x = x_true[0:3]
            # ---------------------------------------------------------------------
            # Simulation part
            # ---------------------------------------------------------------------
            di_k = sqrt(((x[0] - arr_a_i[0, :]) ** 2) + ((x[1] - arr_a_i[1, :]) ** 2) + ((x[2] - arr_a_i[2, :]) ** 2))
            di_k = array([di_k]).T
            if noise_distribution == "normal":
                # See if user passed the mean and std
                if size(arr_distribution_parameters) == 2:
                    mean = arr_distribution_parameters[0]
                    std = arr_distribution_parameters[1]
                    noise_model = gen.normal(loc=mean, scale=std, size=(n, k))
                else:
                    noise_model = gen.normal(size=(n, k))
                di_k = di_k + noise_model
            elif noise_distribution == "exponential":
                # See if user passed the rate
                if size(arr_distribution_parameters) == 1:
                    rate = arr_distribution_parameters[0]
                    noise_model = gen.exponential(scale=rate, size=(n, k))
                else:
                    rate = 10**-6
                    noise_model = gen.exponential(scale=rate, size=(n, k))
                di_k = di_k + noise_model
            elif noise_distribution == "standard_normal":  # Default distribution
                noise_model = gen.standard_normal(size=(n, k))
                di_k = di_k + noise_model
            d_i = median(di_k, axis=1)
            d_i = array([d_i]).T
            # ---------------------------------------------------------------------
            # Estimation part
            # ---------------------------------------------------------------------
            xi_est = zeros(shape=(3, n))
            phi_i = zeros(shape=(n, 1))
            alpha_i = zeros(shape=(n, 1))
            for ii in range(0, n):
                kk = [ii + 1]
                for jj in range(0, n):
                    total = 0
                    if arr_a_i[0, ii] == arr_a_i[0, jj]:
                        total += 1
                    if arr_a_i[1, ii] == arr_a_i[1, jj]:
                        total += 1
                    if ii != jj and total > 0:
                        kk.append(jj + 1)
                a2 = zeros(shape=(len(array(list(itertools.combinations(kk, 2)))), 3))
                b2 = zeros(shape=(len(array(list(itertools.combinations(kk, 2)))), 1))
                for uu in range(0, len(array(list(itertools.combinations(kk, 2))))):
                    combinations = array(list(itertools.combinations(kk, 2)))
                    gg = combinations[uu, 0]
                    hh = combinations[uu, 1]
                    a2[uu, :] = 2 * (arr_a_i[0:3, gg - 1] - arr_a_i[0:3, hh - 1]).T
                    b2[uu] = (
                        d_i[hh - 1] ** 2
                        - d_i[gg - 1] ** 2
                        - norm(arr_a_i[0:3, hh - 1]) ** 2
                        + norm(arr_a_i[0:3, gg - 1]) ** 2
                    )
                xi_est[:, [ii]] = solve(dot(a2.T, a2) + (1 * 10 ** (-6)) * eye(3), dot(a2.T, b2))
                di_xy = norm(xi_est[0:2, 0])
                xi_est[2][ii] = (cmath.sqrt((d_i[0] ** 2)[0] - (di_xy**2)).real) + (
                    cmath.sqrt((d_i[0] ** 2)[0] - (di_xy**2)).imag
                )
                phi_i[ii] = (
                    math.atan2((xi_est[1][ii] - arr_a_i[1, ii]), (xi_est[0][ii] - arr_a_i[0, ii])) * 180 / math.pi
                )
                alpha_i[ii] = (
                    math.acos((xi_est[2][ii] - arr_a_i[2, ii]) / (norm(xi_est[:, ii] - arr_a_i[:, ii]))) * 180 / math.pi
                )
            u_i_1 = cos((phi_i * math.pi) / 180).T
            u_i_2 = sin((alpha_i * math.pi) / 180).T
            u_i_3 = sin((phi_i * math.pi) / 180).T
            u_i_4 = cos((alpha_i * math.pi) / 180).T
            u_i = zeros(shape=(3, n))
            u_i[0, :] = u_i_1 * u_i_2
            u_i[1, :] = u_i_3 * u_i_2
            u_i[2, :] = u_i_4
            a_1 = u_i.T
            b_1 = d_i + (sum(u_i * arr_a_i).T.reshape(n, 1))
            w_i = asarray((1 / d_i) / (sum(1 / d_i)))
            w = asarray(eye(n) * scimath.sqrt(w_i))
            a_loc = dot(w, a_1)
            b_loc = dot(w, b_1)
            x_est = asarray(solve(dot(a_loc.T, a_loc) + (1 * 10 ** (-6)) * eye(3), dot(a_loc.T, b_loc)))
            estimated_positions.append(x_est[:, 0])
            true_trajectory.append(x_true.copy())
            uav_velocity = _velocity(x_est[:, 0], arr_destinations[ww, :], v_max, tau, gamma)
            x_true[0] = x_true[0] + uav_velocity[0]
            x_true[1] = x_true[1] + uav_velocity[1]
            x_true[2] = x_true[2] + uav_velocity[2]
            distance = math.sqrt(
                (x_true[0] - arr_destinations[ww][0]) ** 2
                + (x_true[1] - arr_destinations[ww][1]) ** 2
                + (x_true[2] - arr_destinations[ww][2]) ** 2
            )
        ww += 1
    return array([array(estimated_positions), array(true_trajectory)])

Plots

This module contains the plotting functions.

plot_rmse 

plot_rmse(
    estimated_trajectories: List[ArrayLike],
    true_trajectories: List[ArrayLike],
    names_of_the_algorithms: Optional[List[str]] = None,
) -> NDArray

Plots the root mean squared error along the trajectory for one or more algorithms.

Parameters:

Name Type Description Default
estimated_trajectories List[ArrayLike]

The estimated trajectory that the UAV followed.

 required
true_trajectories List[ArrayLike]

The true trajectory that the UAV followed.

 required
names_of_the_algorithms Optional[List[str]]

The names of the algorithms in the same order as in estimated_trajectories.

 None

Returns:

Type Description
NDArray

An NDArray object containing the RMSE comparison.
Source code in autonav/plots.py 
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
def plot_rmse(
    estimated_trajectories: List[ArrayLike],
    true_trajectories: List[ArrayLike],
    names_of_the_algorithms: Optional[List[str]] = None,
) -> NDArray:
    """Plots the root mean squared error along the trajectory for one or more algorithms.

    Args:
       estimated_trajectories: The estimated trajectory that the UAV followed.
       true_trajectories: The true trajectory that the UAV followed.
       names_of_the_algorithms: The names of the algorithms in the same order as in estimated_trajectories.

    Returns:
        An NDArray object containing the RMSE comparison.
    """
    # User didn't input names_of_the_algorithms
    if names_of_the_algorithms is None:
        names_of_the_algorithms = ["GTRS", "WLS"]
    if len(estimated_trajectories) == len(true_trajectories):
        fig, axs = plt.subplots(len(estimated_trajectories), sharey=True)
        # Space between subplots
        fig.tight_layout(pad=5.0)
        for i in range(len(estimated_trajectories)):
            # Transform inputs in NDArray
            arr_estimated_trajectories: NDArray = asarray(estimated_trajectories[i], dtype=float)
            arr_true_trajectories: NDArray = asarray(true_trajectories[i], dtype=float)
            rmse = compute_rmse(arr_estimated_trajectories[:, :], arr_true_trajectories[:, :])
            axs[i].plot(rmse)
            axs[i].set_title("RMSE " + names_of_the_algorithms[i])
        for ax in axs.flat:
            ax.set(xlabel="Iteration", ylabel="RMSE")
        return axs
    else:
        raise ValueError("The number of algorithms must be the same in estimated_trajectories and true_trajectories.")

plot_trajectories 

plot_trajectories(
    ideal_trajectory: ArrayLike,
    estimated_trajectories: List[ArrayLike],
    a_i: ArrayLike,
    names_of_the_algorithms: Optional[List[str]] = None,
) -> list

Plots the ideal and estimated trajectory for one or more algorithms.

Parameters:

Name Type Description Default
ideal_trajectory ArrayLike

The ideal trajectory that the UAV is supposed to follow.

 required
estimated_trajectories List[ArrayLike]

The estimated trajectory that the UAV followed using an algorithm.

 required
names_of_the_algorithms Optional[List[str]]

The names of the algorithms in the same order as in estimated_trajectories.

 None
a_i ArrayLike

The position of the anchors.

 required

Returns:

Type Description
list

A list of Matplotlib Axes object containing the ideal and estimated trajectory comparison.
Source code in autonav/plots.py 
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
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
def plot_trajectories(
    ideal_trajectory: ArrayLike,
    estimated_trajectories: List[ArrayLike],
    a_i: ArrayLike,
    names_of_the_algorithms: Optional[List[str]] = None,
) -> list:
    """Plots the ideal and estimated trajectory for one or more algorithms.

    Args:
        ideal_trajectory: The ideal trajectory that the UAV is supposed to follow.
        estimated_trajectories: The estimated trajectory that the UAV followed using an algorithm.
        names_of_the_algorithms: The names of the algorithms in the same order as in estimated_trajectories.
        a_i: The position of the anchors.

    Returns:
        A list of Matplotlib Axes object containing the ideal and estimated trajectory comparison.
    """
    # Transform inputs in NDArray
    arr_ideal_trajectory: NDArray = asarray(ideal_trajectory, dtype=float)
    arr_a_i: NDArray = asarray(a_i, dtype=float)
    # User didn't input names_of_the_algorithms
    if names_of_the_algorithms is None:
        names_of_the_algorithms = ["GTRS", "WLS"]
    if len(estimated_trajectories) == len(names_of_the_algorithms):
        axes = []
        for j in range(len(estimated_trajectories)):
            # Transform inputs in NDArray
            arr_estimated_trajectories: NDArray = asarray(estimated_trajectories[j], dtype=float)
            plt.figure(j)  # New figure foreach algorithm
            ax = plt.axes(projection="3d")
            a_i_label = "a_i"
            for i in range(0, arr_a_i.shape[1]):
                ax.plot(
                    arr_a_i[0][i],
                    arr_a_i[1][i],
                    arr_a_i[2][i],
                    "s",
                    markersize=10,
                    markeredgecolor="black",
                    markerfacecolor="black",
                    label=a_i_label,
                )
                a_i_label = "_nolegend_"  # Legend only in the first iteration
            ax.plot(
                arr_ideal_trajectory[:, 0],
                arr_ideal_trajectory[:, 1],
                arr_ideal_trajectory[:, 2],
                color="green",
                label="Ideal Trajectory",
                linewidth=3.0,
                alpha=0.7,
            )
            ax.plot(
                arr_estimated_trajectories[:, 0],
                arr_estimated_trajectories[:, 1],
                arr_estimated_trajectories[:, 2],
                label="Trajectory followed by the UAV using " + names_of_the_algorithms[j],
                color="red",
                alpha=1.0,
            )
            ax.set_xlabel("Width (m)")
            ax.set_ylabel("Length (m)")
            ax.set(zlabel="Heigth (m)")
            ax.legend()
            axes.append(ax)
        return axes
    else:
        raise ValueError(
            "The number of algorithms must be the same in estimated_trajectories and names_of_the_algorithms."
        )

Metrics

This module contains the metrics functions.

compute_armse 

compute_armse(
    estimated_trajectory: ArrayLike, true_trajectory: ArrayLike
) -> float

This function computes the average root mean squared error between the true and estimated trajectory of the UAV.

Parameters:

Name Type Description Default
estimated_trajectory ArrayLike

The estimated trajectory by the algorithm.

 required
true_trajectory ArrayLike

The true trajectory that the UAV followed.

 required

Returns:

Type Description
float

The average root mean squared error between the true and the estimated trajectories.
Source code in autonav/metrics.py 
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
def compute_armse(estimated_trajectory: ArrayLike, true_trajectory: ArrayLike) -> float:
    """This function computes the average root mean squared error between the true and estimated trajectory of the UAV.

    Args:
        estimated_trajectory: The estimated trajectory by the algorithm.
        true_trajectory: The true trajectory that the UAV followed.

    Returns:
        The average root mean squared error between the true and the estimated trajectories.
    """
    # Transform inputs in NDArray
    arr_estimated_trajectory: NDArray = asarray(estimated_trajectory, dtype=float)
    arr_true_trajectory: NDArray = asarray(true_trajectory, dtype=float)
    rmse = compute_rmse(arr_estimated_trajectory, arr_true_trajectory)
    if len(rmse) != 0:
        armse = sum(rmse) / len(rmse)
    else:
        raise ZeroDivisionError("RMSE is empty!")
    return armse

compute_rmse 

compute_rmse(
    estimated_trajectory: ArrayLike, true_trajectory: ArrayLike
) -> list

Computes the root mean squared error between the true and estimated trajectory of the UAV.

Parameters:

Name Type Description Default
estimated_trajectory ArrayLike

The estimated trajectory by the algorithm.

 required
true_trajectory ArrayLike

The true trajectory that the UAV followed.

 required

Returns:

Type Description
list

The average root mean squared error between the true and the estimated trajectories.
Source code in autonav/metrics.py 
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
def compute_rmse(estimated_trajectory: ArrayLike, true_trajectory: ArrayLike) -> list:
    """Computes the root mean squared error between the true and estimated trajectory of the UAV.

    Args:
        estimated_trajectory: The estimated trajectory by the algorithm.
        true_trajectory: The true trajectory that the UAV followed.

    Returns:
        The average root mean squared error between the true and the estimated trajectories.
    """
    # Transform inputs in NDArray
    arr_estimated_trajectory: NDArray = asarray(estimated_trajectory, dtype=float)
    arr_true_trajectory: NDArray = asarray(true_trajectory, dtype=float)
    rmse = []
    # Trajectories must have the same length for comparison
    if size(arr_estimated_trajectory) == size(arr_true_trajectory):
        for i in range(len(arr_estimated_trajectory)):
            norm = sqrt(
                (arr_true_trajectory[i][0] - arr_estimated_trajectory[i][0]) ** 2
                + (arr_true_trajectory[i][1] - arr_estimated_trajectory[i][1]) ** 2
                + (arr_true_trajectory[i][2] - arr_estimated_trajectory[i][2]) ** 2
            )
            rmse.append(sqrt(norm))
    return rmse

Other functions

This module contains the functions to read the path file.

readpathfile 

readpathfile(filename: str) -> NDArray

Reads the path file.

Parameters:

Name Type Description Default
filename str

The name of the file to read.

 required

Returns:

Type Description
NDArray

The waypoints needed to guide the drone.
Source code in autonav/file_handlers.py 
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
def readpathfile(filename: str) -> NDArray:
    """Reads the path file.

    Args:
        filename: The name of the file to read.

    Returns:
        The waypoints needed to guide the drone.
    """
    positions = []
    if os.path.isfile(filename):  # See if file exists
        with open(filename, "r") as file:
            csv_file = csv.reader(file)
            for line in csv_file:
                positions.append([float(x) for x in line])
    else:
        raise FileNotFoundError("File not found, please check path.") from None
    return array(positions)