├── Position.png ├── Velocity.png ├── Acceleration.png ├── Control_scheme.png ├── Robot_Control_tutorial.pdf ├── maxfig.m ├── mydialog.m ├── Kalman_Filter.m ├── README.md └── main.m /Position.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SaysWis/Robot_Control/HEAD/Position.png -------------------------------------------------------------------------------- /Velocity.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SaysWis/Robot_Control/HEAD/Velocity.png -------------------------------------------------------------------------------- /Acceleration.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SaysWis/Robot_Control/HEAD/Acceleration.png -------------------------------------------------------------------------------- /Control_scheme.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SaysWis/Robot_Control/HEAD/Control_scheme.png -------------------------------------------------------------------------------- /Robot_Control_tutorial.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SaysWis/Robot_Control/HEAD/Robot_Control_tutorial.pdf -------------------------------------------------------------------------------- /maxfig.m: -------------------------------------------------------------------------------- 1 | figure 2 | % find screen size 3 | screensize = get( 0, 'ScreenSize' ); 4 | % set figure to screen size 5 | set(gcf,'Position',screensize) 6 | -------------------------------------------------------------------------------- /mydialog.m: -------------------------------------------------------------------------------- 1 | function[Kp,Ki]= mydialog 2 | prompt = {'Proportional Gain (Kp):','Integral Gain (Ki):'}; 3 | dlgtitle = 'PID Controller Gains'; 4 | dims = [1 50; 1 50]; 5 | definput = {'0.8','0.001'}; 6 | answer = inputdlg(prompt,dlgtitle,dims,definput); 7 | values = str2double(answer); 8 | 9 | %% 10 | Kp = values(1); 11 | Ki = values(2); 12 | end -------------------------------------------------------------------------------- /Kalman_Filter.m: -------------------------------------------------------------------------------- 1 | function [x_up, P_up] = Kalman_Filter(x_k_1, P_k_1, y, F, H, Q, R) 2 | 3 | x_k = F*x_k_1; 4 | P_k = F*P_k_1*F'+Q; % Project the State Covariance Ahead 5 | 6 | Kk = P_k*H'*inv(H*P_k*H'+R); % Kalman Gain 7 | x_up = x_k+Kk*(y-H*x_k); % Update Measurement with measurement 8 | P_up = P_k-Kk*H*P_k; % Update the Error Covariance 9 | 10 | end -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Robot Control using PID Controller and Kalman Filter 2 | The objective is to estimate the Velocity using a Linear Kalman Filter and to control a robot using a PID Controller. 3 | 4 | Consider a robot on frictionless, straight lane. Initially, the robot is stationary at position 0. We measure the position of the robot every $\Delta t$ seconds, but these measurements are imprecise; we want to maintain a model of the robot's position and velocity. 5 | 6 | We show here how we derive the model from which we create our Kalman filter. 7 | Since $F$, $H$ , $R$ and $Q$ are constant, their time indices are dropped. 8 | 9 | The position and velocity of the robot are described by the linear state space 10 | $x_k$ = [ $x$ , $v$ ] % $x$ : position 11 | ; $v$ : velocity 12 | 13 | We assume that a(k) is unknown and normally distributed with mean 0 and standard deviation $\sigma_a$. From Newton's laws of motion we conclude that 14 | $$x(k) = F x(k-1) + G a(k)$$ 15 | 16 | We suppose there is no control inputs $G a(k)$ term, where 17 | $$F = [1 \quad Δt;0 \quad 1]$$ 18 | $$G = [Δt^2/2;Δt]$$ 19 | 20 | 21 | Then, a PID controller is used to generates the acceleration ( $a$ ) to control the robot's velocity ( $v$ ) using the estimated one ( $\tilde{v}$ ). 22 | 23 |

24 | 25 |

26 | 27 | 28 | ``` 29 | To try this example ... 30 | 1. Run the main.m file 31 | 2. A pop-up message will appear 32 | 3. Choose your tuning PID Controller parameter (by default Kp = 0.8 and Ki = 0.001) 33 | 4. That's it ! 34 | ``` 35 | -------------------------------------------------------------------------------- /main.m: -------------------------------------------------------------------------------- 1 | %% Toy example link : https://en.wikipedia.org/wiki/Kalman_filter#Example_application.2C_technical 2 | clear 3 | clc 4 | 5 | %% Initialization 6 | Ts = 0.1; % Sampling Time 7 | time=0:Ts:100; 8 | N = length(time)-1; 9 | 10 | % For Plant 11 | x0 = [0;0]; % Position, Velocity 12 | x(:,1) = x0; 13 | 14 | % For Kalman Filter 15 | sigma_a = 0.1; 16 | sigma_y = 0.01; 17 | mu=0; 18 | x_hat(:,1) = x0; 19 | P_k{1} = 0*eye(2,2); 20 | P_k_norm(1) = norm(P_k{1}); 21 | 22 | % Velocity Profile 23 | Velocity(1:20) = 0; % m/s 24 | Velocity(20:100) = 0.5; 25 | Velocity(100:300) = 1.2; 26 | Velocity(300:500) = 1; 27 | Velocity(500:700) = 0; 28 | Velocity(700:800) = 0.7; 29 | Velocity(800:1001) = 0; 30 | 31 | %% Plant Model 32 | F = [1 Ts; 0 1]; 33 | G = [Ts^2/2; Ts]; 34 | H = [1 0]; % we have only position's measurement 35 | 36 | % Check System Controllability 37 | if(rank(ctrb(F,G))==2) 38 | disp('System is controllable !') 39 | end 40 | % Check System Observability 41 | if(rank(obsv(F,H))==2) 42 | disp('System is observable !') 43 | end 44 | 45 | %% PID Controller 46 | Kp = 0.8; 47 | Ki = 1e-3; 48 | 49 | [Kp,Ki]= mydialog; 50 | %% Velocity Estimate using Kalman Filter 51 | Q = G*G'*sigma_a^2; 52 | R = sigma_y^2; 53 | noise = sigma_y*randn(1,N+1)+mu; % Generate White Noise 54 | 55 | Error = zeros(1,length(N+1)); 56 | Prop = zeros(1,length(N+1)); 57 | Integral = zeros(1,length(N+1)); 58 | 59 | for t=1:N 60 | % PID Controller 61 | Error(t+1) = Velocity(t)-x_hat(2,t); 62 | 63 | Prop(t+1) = Error(t+1); 64 | Int(t+1)=Error(t+1)*Ts; 65 | Integral(t+1)= sum(Int); 66 | 67 | a(t+1) = Kp*Prop(t+1)+ Ki*Integral(t+1); 68 | 69 | % Plant Open Loop Simulation 70 | x(:,t+1) = F*x(:,t) + G*a(t+1); 71 | y(t+1) = H*x(:,t+1)+ noise(t+1); % Add noise to the measurement 72 | 73 | % Estimate the Velocity using Kalman Filter 74 | [x_up, P_k{t+1}] = Kalman_Filter(x_hat(:,t), P_k{t}, y(t), F, H, Q, R); 75 | x_hat(:,t+1) = x_up; 76 | end 77 | 78 | %% Plot Data 79 | maxfig 80 | subplot(2,1,1) 81 | plot(time,x(1,:),'-',time,x_hat(1,:),'--',time, y,'--','Linewidth',2) 82 | xlabel('Time (s)','fontweight','bold') 83 | ylabel('Distance (m)','fontweight','bold') 84 | grid on 85 | legend('True Position','Estimated Position','Measurement','Location','northwest') 86 | title('Estimated vs Real Position', 'FontSize', 14) 87 | axes('position',[.68 .64 .2 .2]) 88 | box on 89 | your_index = 58