## Problem 1 - Linear Quadratic Regulator

Problem Statement:

Consider a tracking sensor which in steady state tracks an object. Let θ(t) be the angle (in radians) between the direction the sensor is pointing, and the direction of the object being tracked. If the sensor loses the track (i.e., θ(t) ≠ 0), in the absence of any feedback control to return the sensor to the object, the sensor will reorient itself to a given reference orientation. Assume this behavior may be modeled using:

θ_nc (t+1)= 1.2*θ_nc (t).

where “nc” means “no control.”

We want to build a Linear Quadradic Regulator (LQR) that will optimally return the system to tracking an object given it has lost track. Note: the control input provides the correction required to bring the sensor back into track (e.g., from a Kalman Filter). MATLAB Code: problem_1.m

Author: Yash Bansod
Date: 2nd May, 2020
Problem 1 - Linear Quadratic Regulator

GitHub: https://github.com/YashBansod

### Clear the environment and the command line

``````clear;
close all;
clc;
``````

### Define the input parameters

``````init_theta = 0.05;
delta_t = 1;                     % Delta time
num_steps = 61;                     % Number of time steps (including t=0)

R = 1;
Q = 3;
``````

### LQR computations

``````theta = zeros(1, num_steps);
theta_nc = zeros(1, num_steps);
u = zeros(1, num_steps);
K = zeros(1, num_steps);
P = zeros(1, num_steps);

A = 1.2;
B = 1;

theta(1) = init_theta;
theta_nc(1) = init_theta;
P(end) = Q;

for t_step = num_steps:-delta_t:2
P(t_step - 1) = Q + (A * P(t_step) * A') - (A' * P(t_step) * B * ...
inv(R + B' * P(t_step) * B) * B' * P(t_step) * A);
end

for t_step = 1:delta_t:num_steps-1
K(t_step) = -inv(R + B' * P(t_step + 1) * B) *  B' * P(t_step + 1) * A;
u(t_step) = K(t_step) * theta(t_step);
theta(t_step + 1) = A * theta(t_step) + B * u(t_step);
theta_nc(t_step + 1) = A * theta_nc(t_step);
end
``````

### Plot the results

``````time_line = 0:delta_t:(num_steps - 1)* delta_t;

figure(1);

subplot(3, 1, 1)
plot(time_line, theta);
hold on;
grid on;
plot(time_line, u);
title('Theta(t) and U(t)');
xlabel('Time (seconds)');
ylabel('Value');
legend('Theta(t)', 'U(t)')

subplot(3, 1, 2)
plot(time_line, K);
grid on;
title('K(t)');
xlabel('Time (seconds)');
ylabel('Value');

subplot(3, 1, 3)
plot(time_line, theta_nc);
grid on;
title('Theta_N_C(t)');
xlabel('Time (seconds)');
ylabel('Value');
`````` 