Design the controller for the system ~(\ref{equ7}) 
\begin{align}\label{equ21}
	\omega \nonumber &= \frac{\mathcal{Q}(\bar{p}_e)}{\rho}\left(u+\frac{(r-\rho) \frac{u}{r}} {\rho k_1 (k_a^2 - z_e^2)} + u\sigma(r\dot{r})\right) +\\ 
	 &\hspace{1.8cm} \frac{(1-\mathcal{Q}(\bar{p}_e))}{\rho}\left(u+\frac{(r-\rho) \frac{u}{r}} {\rho k_1 (k_b^2 - z_e^2)} + u\sigma(r\dot{r})\right), 
\end{align}
where $k_1$,  $k_a$ and $k_b$ is any positive constant. Then origin of the closed-loop system is asymptotically stable with constraint $z_e$ is \( -k_b< z_e < k_a \).
\end{theorem}
\begin{proof}
Substitute the value of design controller $\omega $ in the system equation(\ref{equ7}), if transformed error term $ 0 < z_e < k_a$ the behaviors of the system are:
\begin{align}\label{equ22}
	\nonumber \dot{\bar{p}}_x& = \frac{u \bar{p}_y}{\rho} - \left(1-\frac{\bar{p}_y}{\rho}\right)\left(\frac{(r-\rho) \frac{u}{r}} {\rho k_1 (k_a^2 - z_e^2)} +  u\sigma(u\bar{p}_x)\right),\\
	\dot{\bar{p}}_y& = -\frac{\bar{p}_x}{\rho}\left(u+\frac{(r-\rho) \frac{u}{r}} {\rho k_1 (k_a^2 - z_e^2)} + u\sigma(u\bar{p}_x)\right).
\end{align}

Similarly, when transformed error term $ -k_b < z_e \leq 0$ ,
\begin{align}\label{equ23}
	\nonumber \dot{\bar{p}}_x& = \frac{u \bar{p}_y}{\rho} - \left(1-\frac{\bar{p}_y}{\rho}\right)\left(\frac{(r-\rho) \frac{u}{r}} {\rho k_1 (k_b^2 - z_e^2)} +  u\sigma(u\bar{p}_x)\right),\\
	\dot{\bar{p}}_y& = -\frac{\bar{p}_x}{\rho}\left(u+\frac{(r-\rho) \frac{u}{r}} {\rho k_1 (k_b^2 - z_e^2)} + u\sigma(u\bar{p}_x)\right).
\end{align}  
Substitute the value of design controller $\omega$  ~(\ref{equ21}) in ~(\ref{equ19}), while the transformed error term \( 0 < z_e < k_a\) than  \(\dot{V}\) is obtained,
\begin{align}\label{equ24}
	\dot{V} &= \frac{1}{\rho k_1} \frac{(r - \rho) \frac{u \bar{p}_x} {r}}{k_a^2 - z_e^2}+\bar{p}_x \left( u - \rho \left( \frac{1}{\rho} \left( u \right. \right. \right. \nonumber \\
	& \hspace{2.7cm} \left. \left. \left. + \frac{(r - \rho)\frac{u}{r}}{\rho k_1 (k_a^2 - z_e^2)} + u\sigma(u\bar{p}_x) \right) \right) \right).
\end{align}

Similarly, substitute the value of design controller $\omega$ in ~(\ref{equ19}), while the transformed error term \( -k_b <z_e \leq 0\) than  \(\dot{V}\) is obtained,
\begin{align}\label{equ25}
	\dot{V} &= \frac{1}{\rho k_1} \frac{(r - \rho) \frac{u \bar{p}_x} {r}}{k_b^2 - z_e^2}+\bar{p}_x \left( u - \rho \left( \frac{1}{\rho} \left( u \right. \right. \right. \nonumber \\
	& \hspace{2.7cm} \left. \left. \left. + \frac{(r - \rho)\frac{u}{r}}{\rho k_1 (k_b^2 - z_e^2)} + u\sigma(u\bar{p}_x) \right) \right) \right).
\end{align}



 Further, simplification of the ~(\ref{equ24}) and ~(\ref{equ25}) equation shows that origin of closed-loop system  is stable

 \begin{align}\label{equ26}
 	\dot{V}  = -u\bar{p}_x\sigma(u\bar{p}_x) \leq 0.
 \end{align}


In the above Lyapunov function \(V\), which is positive definite. Let's say that \(\dot{V}\) is negative semi-definite. 
For show the asymptotically stable, we use the lasalle's invariance principle, to prove asymptotic stability, we need to find the set where \(\dot{V} = 0\), i.e., the set where:

\[
u \bar{p}_x \sigma(u \bar{p}_x) = 0
\]

This implies that \( \bar{p}_x = 0 \) and \( u \neq 0 \). Using this, we analyze the system given by equations~(\ref{equ22}, \ref{equ23}), leading to the conclusion that \( \bar{p}_y = 0 \). Then origin of the closed-loop system is asymptotically stable with constraint $z_e$ is \( -k_b< z_e < k_a \).
\end{proof}
\subsection{No availability of the range rate}
The controller~(\ref{equ21}) achieves target circumnavigation, but its implementation requires explicit differentiation of distance measurements(range rate) and may increase noise. We must consider the scenario where only distance is available. To this purpose, we designate the output of the system~(\ref{equ4}) as \( y = 0.5r^2 \) and suggest a dynamic output feedback controller:
\begin{align}\label{equ27}
	\omega \nonumber &= \frac{\mathcal{Q}(\bar{p}_e)}{\rho}\left(u+\frac{(r-\rho) \frac{u}{r}} {\rho k_1 (k_a^2 - z_e^2)} + u\sigma(-k_2 z + y)\right) +\nonumber \\  
	& \frac{(1-\mathcal{Q}(\bar{p}_e))}{\rho}\left(u+\frac{(r-\rho) \frac{u}{r}} {\rho k_1 (k_b^2 - z_e^2)} + u\sigma(-k_2 z + y)\right), \nonumber \\  
	 \dot{z} &= -k_2 z + y,
\end{align}
Where \( k_1 \) and \( k_2 \) are constants with arbitrary positive values. We let \( \varphi = -k_2 z + y \) to make analysis easier.

Now calculate the time derivative of \(\varphi\) 
\[
\dot{\varphi}= -k_2\dot{z} + r\dot{r}.
\]
Substituting the $\dot{z}$ from~(\ref{equ27}) and $r\dot{r}$ from~(\ref{equ10}) into the above time derivative expression, finally we get:
\[
\dot{\varphi} = -k_2 \varphi + u \bar{p}_x.
\]

\begin{remark}
	The primary concept behind the control law~(\ref{equ27}) is to employ an observer that uses only the distance and the dynamic equation of \( z \) to estimate \( r\dot{r} \). This estimate, \( \varphi \), then eliminate \( r\dot{r} \) in equation~(\ref{equ10}). While the equation for \( z \) resembles a high-gain reduced-order observer, it has notable differences: the gain \( k_2 \) does not need to be very large. Additionally, the output feedback controller ensures the global stability of the closed-loop system.
\end{remark}
Now substituting the equation~(\ref{equ27}) into~(\ref{equ7})and If transformed error term \( 0 < z_e < k_a\) than then we get: 
\begin{align}\label{equ28}
	\nonumber \dot{\bar{p}}_x &= -dk_1 u \sigma(u\bar{p}_x) + \left( \frac{-k_1 u \sigma(\varphi)}{k_a^2 - \bar{p}_k^2} - \frac{u}{\rho} \right) \bar{p}_y,\\
	\nonumber\dot{\bar{p}}_y &= \left( \frac{k_1 u \sigma(\varphi)}{k_a^2 - \bar{p}_k^2} + \frac{u}{\rho} \right) \bar{p}_x,\\
	\dot{\varphi} &= -k_2 \varphi + u \bar{p}_x.
\end{align}
If transformed error term \( -k_b< z_e \leq 0\) than system equation (\ref{equ7}),
\begin{align}\label{equ29}
	\nonumber \dot{\bar{p}}_x &= -dk_1 u \sigma(u\bar{p}_x) + \left( \frac{-k_1 u \sigma(\varphi)}{k_b^2 - \bar{p}_k^2} - \frac{u}{\rho} \right) \bar{p}_y,\\
	\nonumber\dot{\bar{p}}_y &= \left( \frac{k_1 u \sigma(\varphi)}{k_b^2 - \bar{p}_k^2} + \frac{u}{\rho} \right) \bar{p}_x,\\
	\dot{\varphi} &= -k_2 \varphi + u \bar{p}_x.
\end{align}
\begin{theorem}\label{without_rangerate}
	The origin of the closed-loop system ~(\ref{equ28},\ref{equ29}) is asymptotically stable with constraint $z_e$ is \( -k_b< z_e < k_a \).
\end{theorem} 

\textbf{Proof:} Consider a composite Lyapunov function candidate that is radially unbounded:
\begin{align}\label{equ30}
V_r(\bar{e}_x, \bar{e}_y, \varphi) = V  + \int_0^\varphi \sigma(\tau) \, d\tau.
\end{align}
The time derivative of \( V_r \), evaluated along the trajectory of system ~(\ref{equ28}) is:
\begin{align}\label{equ}
\dot{V}_r(\bar{e}_x, \bar{e}_y, \varphi) =\dot{V}  + \frac{d(\int_0^\varphi \sigma(\tau) \, d\tau)}{dt}.
\end{align}
After the substitute the value of $\dot{V}$ in above equation
\begin{align}\label{equ31}
	\dot{V}_r \nonumber &=\frac{\mathcal{Q}(z_e)}{\rho k_1} \frac{(r-\rho)\frac{u \bar{p}_x}{r}} {k_a^2 - z_e^2}+\frac{(1-\mathcal{Q}(z_e))}{\rho k_1} \frac{(r-\rho)\frac{u \bar{p}_x}{r}} {k_b^2 - z_e^2}\\
&\hspace{3.0cm} + \bar{p}_x(u-\rho \omega) +\dot{\varphi}\sigma(\varphi).
\end{align}




Substitute the value of design controller $\omega$ ~(\ref{equ27}) in ~(\ref{equ31}), while the transformed error term \( 0 < z_e < k_a\) than  \(\dot{V}\) is obtained,
\begin{align}\label{equ32}
	\dot{V}_r\nonumber &= \frac{1}{\rho k_1} \frac{(r - \rho) \frac{u \bar{p}_x} {r}}{k_a^2 - z_e^2}+\bar{p}_x \left( u - \rho \left( \frac{1}{\rho} \left( u \right. \right. \right. \nonumber \\
	& \hspace{1.5cm} \left. \left. \left. + \frac{(r - \rho)\frac{u}{r}}{\rho k_1 (k_a^2 - z_e^2)} + u\sigma(\varphi) \right) \right) \right) +\dot{\varphi}\sigma(\varphi).
\end{align}

Similarly, put the value of design controller  $\omega$ ~(\ref{equ27}) in ~(\ref{equ31}), while the transformed error term \( -k_b < z_e \leq 0\) than  \(\dot{V}\) is obtained,
\begin{align}\label{equ33}
	\dot{V}_r \nonumber &= \frac{1}{\rho k_1} \frac{(r - \rho) \frac{u \bar{p}_x} {r}}{k_b^2 - z_e^2}+\bar{p}_x \left( u - \rho \left( \frac{1}{\rho} \left( u \right. \right. \right. \nonumber \\
	& \hspace{1.5cm} \left. \left. \left. + \frac{(r - \rho)\frac{u}{r}}{\rho k_1 (k_b^2 - z_e^2)} + u\sigma(\varphi) \right) \right) \right)+\dot{\varphi}\sigma(\varphi).
\end{align}



Further, simplification of the ~(\ref{equ32}) and ~(\ref{equ33}) 
\[
\dot{V}_r = -k_2 \varphi \sigma(\varphi) \leq 0.
\]
This confirms that the Lyapunov function is decreasing, thereby ensuring the stability of the system.In the above Lyapunov function \(V\), which is positive definite. Let's say that \(\dot{V}\) is negative semi-definite. 
For show the asymptotically stable, we use the lasalle's invariance principle, to prove asymptotic stability, we need to find the set where \(\dot{V} = 0\), i.e., the set where:

\[
\varphi \sigma(\varphi) = 0
\]

This implies that \( \varphi = 0 \) and \( u \neq 0 \). Using this, we analyze the system ~(\ref{equ28}, \ref{equ29}), leading to the conclusion that \( \bar{p}_x = 0, \bar{p}_y = 0 \). Then origin of the closed-loop system is asymptotically stable with constraint $z_e$ is \( -k_b< z_e < k_a \).

\begin{theorem}
	Under the conditions given in Theorem \ref{without_rangerate}, the following statements hold:
	
	\textbf{(a)} The distance error \( z_e = r - \rho \) satisfy:
	\begin{align}\label{equ34}
	\nonumber	-\Upsilon_b \leq z_e & \leq   \Upsilon_a, \\
	\nonumber 	\Upsilon_a  &= k_a \sqrt{1 - e^{-2\rho k_1 V_r(0)}},  \\
  	\Upsilon_b & = k_b \sqrt{1 - e^{-2 \rho k_1 V_r(0)}}, 
	\end{align}
	
	
	\textbf{(b)} The transformed error term remains bounded by 
	\begin{align}
		|p_e| \leq \sqrt{2 V_r(0)}
	\end{align}
	
	
\end{theorem}

\begin{proof}
	
    \textbf{(a)}	 According to Theorem \ref{without_rangerate}, we have:\(V_r \leq V_r (0)\)using this, along with equation  ~(\ref{equ17}) , the ABLF term of composite Lyapunov function~(\ref{equ30}) of the satisfies the inequality:
	\begin{align}
		V_r (0) & \geq
		\begin{cases}
			\frac{1}{2\rho k_1} \ln \left( \frac{k_a^2(t)}{k_a^2 - z_e^2} \right), & \text{if } 0 < z_e < k_a, \\
			\frac{1}{2\rho k_1} \ln \left( \frac{k_b^2(t)}{k_b^2 - z_e^2} \right), & \text{if } -k_b < z_e \leq 0,
		\end{cases}
	\end{align}
	 Taking the exponential of both sides gives:
	\begin{align}
		z_e^2 & \leq 
		\begin{cases}
			k_a^2 \left( 1 - e^{-2\rho k_1 V_r(0)} \right), & \text{if }  0 < z_e < k_a, \\
			k_b^2 \left( 1 - e^{-2 \rho k_1 V_r(0)} \right), & \text{if} -k_b < z_e \leq 0.
		\end{cases}
	\end{align}
	Taking the square root of these inequalities yields:
	\begin{align}
		 z_e  & \leq 
		\begin{cases}
			k_a \sqrt{1 - e^{-2\rho k_1 V_r(0)}}, & \text{if }  0 < z_e < k_a, \\
			-k_b \sqrt{1 - e^{-2\rho k_1 V_r(0)}}, &  \text{if} -k_b < z_e \leq 0.
		\end{cases}
	\end{align}
  Since it is clear that:
	\begin{align}
			-\Upsilon_b \leq z_e & \leq   \Upsilon_a,
	\end{align}
	where \( \Upsilon_a \) and \( \Upsilon_b \) are defined in equation ~(\ref{equ34}).\\
  \textbf{(b)} Applying the same argument as in part (a) for the quadratic Lyapunov function  in equation ~(\ref{equ30}), we deduce:
	\begin{align}
		\frac{1}{2} p_e^2 & \leq V_r(0) \quad \Rightarrow \quad p_e \leq \sqrt{2V_r}(0).
	\end{align}

	thus completing the proof.
	
\end{proof}
clear all;
close all;

% Parameters
xt = 0; 
yt = 0; 
v = 1.0; 
d = 1.0; 
z0 = 0; 
k1 = 1;  
k2 = 2.5; 
% kb = 0.75;
r_i = 0.75;
r_o = 1.75;

% ka = 1;
kb = d-r_i;
ka = r_o -d;
%ka = 0.166;
%kb = 1.56;
%theta = pi/2;
% Initial conditions: [x-position; y-position; heading angle; z]
x0 = [1.05; 0.53; 45*pi/180; z0];
%x0 = [1.49; 0.91; 25*pi/180; z0];
%x0 = [1.45; 0.89; 45*pi/180; z0];
% %x0 = [1.0; 0.75; 90.1*pi/180; z0];
% x0 = [-1; 0.6; 240*pi/180; z0];
tspan = [0 50];


opts = odeset('RelTol',1e-5,'AbsTol',1e-7);

% Solve ODE
f = @(t, x) dynamics_unknown(t, x, v, k1, d, k2, ka,kb);
[t, x] = ode45(f, tspan, x0, opts);
control_terms = arrayfun(@(i) calculate_control_term(x(i, :), x(i, 3), v, k1, d, [], ka, kb), 1:size(x, 1));


%%
% Plot Trajectories and Results
figure(1)
%plot(x(:, 1), x(:, 2), 'b', xt, yt, 'ro', 'LineWidth', 1.2);
plot(x(:, 1), x(:, 2), 'b', xt, yt,'r.', 'LineWidth', 1.2, 'MarkerSize', 15);
hold on;
plot(x(1, 1), x(1, 2), 'LineWidth', 2);
%plot(x(1, 1), x(1, 2), 'rx', 'MarkerSize', 8, 'LineWidth', 2);
% viscircles([xt yt], kb, 'Color', [1  0 0], 'LineWidth', 1);
% viscircles([xt yt], ka+d, 'Color', [1  0 0], 'LineWidth', 1);
viscircles([xt yt], r_i, 'Color', [1  0 0], 'LineWidth', 1.5);
viscircles([xt yt], r_o, 'Color', [1  0 0], 'LineWidth', 1.5);
%rectangle('Position', [xt-r_i, yt-r_i, 2*r_i, 2*r_i], 'Curvature', [1, 1], 'EdgeColor', [1 0 0], 'LineWidth', 1.5, 'LineStyle', '-');
%rectangle('Position', [xt-r_o, yt-r_o, 2*r_o, 2*r_o], 'Curvature', [1, 1], 'EdgeColor', [1 0 0], 'LineWidth', 1.5, 'LineStyle', '-');

% axis([-kb-0.3 kb+0.3 -kb-0.3 kb+0.3]);

% axis([-ka-0.3 ka+0.3 -ka-0.3 ka+0.3]);

xlabel('x (m)','Interpreter','latex','fontsize',16);
ylabel('y (m)','Interpreter','latex','fontsize',16);
legend('Robot', 'Target');

xtraj = x(:,1);
ytraj = x(:,2);
theta=x(:,3);
% axes('position', [0.25 0.25 0.5 0.5]);
% box on;
% alpha = 0 < t & t < 300;
% plot(xtraj(alpha), ytraj(alpha), 'b' , 'LineWidth', 2, 'MarkerSize', 5.5);
% hold on;
% plot(x(1, 1), x(1, 2), 'rx', 'MarkerSize', 8, 'LineWidth', 2);
% viscircles([xt yt], ka, 'Color', [1 0 0], 'LineWidth', 1);
% hold off;
% axis([0.66 1.1 0.45 1.05]);
% grid on;
axis square
axis equal
axis([ -2.5 2.5 -2.5 2.5])
hold off;


%%
figure(2);
plot(t, control_terms, 'm', 'LineWidth', 1.2);
hold on;
xlabel('Time (sec)','Interpreter','latex','fontsize',16);
ylabel('Control Term','Interpreter','latex','fontsize',16);
% axis square
% axis equal
%axis([ -2.5 2.5 -2.5 2.5])
hold off;
%title('Control Term vs Time');
%%

figure(3);
plot(t, x(:, 3), 'g');
xlabel('Time (sec.)','Interpreter','latex','fontsize',15);
ylabel('Heading Angle ($\theta$)','Interpreter','latex','fontsize',15);
%%
const1 = 1.2 * ones(1, length(t));
figure;
% Plot distance to the target over time
r = sqrt((x(:, 1) - xt).^2 + (x(:, 2) - yt).^2); 


plot(t, r, 'g', 'LineWidth', 1.2);
xlabel('Time (sec)');
hold on;
%plot(t, const1, 'r', 'LineWidth', 1.2);
%yline(ka, 'r.', 'LineWidth', 1.4);
ylabel('r (m)');
axis([0 t(end) 0 ka + 0.75]);
legend('r','k_a');

axes('position', [0.25 0.25 0.5 0.5]);
box on;
alpha = 0 < t & t < 300;
plot(t(alpha), r(alpha), 'g' , 'LineWidth', 2, 'MarkerSize', 5.5);
hold on;
plot(t(alpha), const1(alpha), 'LineWidth', 1.4);
hold off;
axis([0 8 1.12 1.22]);
grid on;
hold off;

figure;
plot(t, x(:, 3), 'g');
xlabel('Time (sec)');
ylabel('Heading Angle (\theta)');

r = sqrt((x(:, 1) - xt).^2 + (x(:, 2) - yt).^2); 
figure;
plot(t, r, 'g', 'LineWidth', 1.2);
xlabel('Time (sec)');
hold on;
yline(ka, 'r-.', 'LineWidth', 2);
ylabel('r (m)');
axis([0 t(end) 0 ka + 0.5]);


%k = sqrt((x(:, 1)-d*sin(theta)- xt).^2 + (x(:, 2)+d*cos(theta)- yt).^2); 
pk = r-d;
figure(6);
plot(t, pk, 'g', 'LineWidth', 1.2);
xlabel('Time (sec)','Interpreter','latex','fontsize',16);
ylabel('Error($\bar{p}_k$)','Interpreter','latex','fontsize',16);
% hold on;
% yline(ka, 'r-.', 'LineWidth', 2);
% ylabel('r (m)');
% axis([0 t(end) 0 ka + 0.5]);

% for i=1:length(r)
%     if r(i)>1 && 
%         q=1;
%         blf(i)= q*((1/d)*(v+(((r-d)*(v/r))/(d*k1*(ka^2-(r-d)^2)));
%     else
%         q=0;
%         blf(i)= (1-q)*((1/d)*(v+(((r-d)*(v/r))/(d*k1*(kb^2-(r-d)^2)))+v*c));
%     end
% end
% figure
% plot(t,blf)
% 
%     px = cos(theta)*(x1-d*sin(theta)- 0) + sin(theta)*(y1+d*cos(theta)- 0);
%     pk = r-d;
%    sigma = @(s) s ;
%     c = sigma(v*px);
% for i = 1:length(r)
%     if r(i) > 1
%         q = 1;
%         blf(i) = q * ((1/d) * (v + (((r(i) - d) * (v / r(i))) / (d * k1 * (ka^2 - (r(i) - d)^2)))));
%     else
%         q = 0;
%         blf(i) = (1 - q) * ((1/d) * (v + (((r(i) - d) * (v / r(i))) / (d * k1 * (kb^2 - (r(i) - d)^2)))) + v * c);
%     end
% end
% 
% figure;
% plot(t, blf);



% Function definitions must be at the end of the script
function dxdt = dynamics_unknown(~, x, v, k1, d, k2, ka, kb)
    % States
    x_p = x(1); 
    y_p = x(2); 
    theta = x(3); 
    z = x(4); 

    % Calculate the control term
    control_term = calculate_control_term(x, theta, v, k1, d,k2, ka,kb);
    
    z_dot = -k2 * z + 0.5 * (x_p^2 + y_p^2);

    dxdt = [
        v * cos(theta); % dx/dt
        v * sin(theta); % dy/dt
        control_term;   % dtheta/dt
        z_dot           % dz/dt
    ];
end

function control_term = calculate_control_term(x, theta, v, k1,d, ~,ka, kb)
    % States
    x1 = x(1); 
    y1 = x(2); 
    
    %xp=x1-d*sin(theta);
    %yp=y1+d*cos(theta);

    %xt=0;
    %yt=0;

    %e_x=xp-0;
    %e_y=yp-0;

    
    z = x(4); 
    %ke = sqrt((x1-d*sin(theta)- 0).^2 + (y1+d*cos(theta)- 0).^2); 
    % Error terms
    %e_x = x_p - 0; % Target x-coordinate is 0
    %e_y = y_p - 0; % Target y-coordinate is 0
    %radius = sqrt(e_x^2 + e_y^2 + d^2 + 2*d*(e_x*sin(theta) - e_y*cos(theta)));
    r=sqrt(x1.^2+y1.^2);%&& radius > 1
   % s_fun = sqrt(e_x^2 + e_y^2);
    pk = r-d;
    if pk > 0
         q = 1;
    else
         q = 0;
    end
    px = cos(theta)*(x1-d*sin(theta)- 0) + sin(theta)*(y1+d*cos(theta)- 0);
    % Sigma function
    sigma = @(s) s ;
    c = sigma(v*px);
    control_term = q*((1/d)*(v+(((r-d)*(v/r))/(d*k1*(ka^2-(r-d)^2)))+v*c))+(1-q)*((1/d)*(v+(((r-d)*(v/r))/(d*k1*(kb^2-(r-d)^2)))+v*c));
    %control_term = q*((k1 * v * c / (ka^2 - ke^2)) + (v / d)) + (1 - q) * ((k1 * v * c / (kb^2 - ke^2)) + (v / d));
    %control_term = q*((k1 * v * c / (ka^2 - e_x^2 - e_y^2)) + (v / d)) + (1 - q) * ((k1 * v * c / (kb^2 - e_x^2 - e_y^2)) + (v / d));
end
please provide me code for both secenrio present in range rate and without range rate