Question

Want a (kalman filter implemented in PID paper with a matlab code : m.file).

Answer 1

A PID control is a PD control with another term added, which is proportional to the integral of the process variable.

Adding an integral term causes any remaining steady-state error to build up and enact a change, so a PID controller should be able to track our trajectory (and stabilize the quadcopter) with a significantly smaller steady-state error.

The equation remain identical with just an addition of an extra term:

And this is finally a full PID implementaion:

1. % Compute system inputs and updated state.
2. % Note that input = [g1, . . ., g4]
3. function [input, state] = pid_controller(state, thetadot)
4.         % Controller gains, tuned by hand and intuition.
5.         Kd = 4;
6.         Kp = 3;
7.         Ki = 5.5;
8.         % Initialize the integral if necessary.
9.         if ~isfield(state, ’integral’)
10.                params.integral = zeros(3, 1);
11.                params.integral2 = zeros(3, 1);
12.         end
13.         % Prevent wind-up
14.         if max(abs(params.integral2)) > 0.01
15.                params.integral2(:) = 0;
16.         end
17.         % Compute total thrust
18.         total = state.m * state.g / state.k / (cos(state.integral(1)) * cos(state.integral(22))
19.         % Compute errors
20.         e = Kd * thetadot + Kp * params.integral - Ki * params.integral2;
21.         % Solve for the inputs, gi
22.         input = error2inputs(params, accels, total);
23. % Update the state
24.         params.integral = params.integral + params.dt .* thetadot;
25.         params.integral2 = params.integral2 + params.dt .*    params.integral;
26. end

Full simulation code:

1. % Simulation times, in seconds.
2. start_time = 0;
3. end_time = 10;
4. dt = 0.005;
5. times = start_time:dt:end_time;
6. % Number of points in the simulation.
7. N = numel(times);
8. % Initial simulation state.
9. x = [0; 0; 10];
10. xdot = zeros(3, 1);
11. theta = zeros(3, 1);
12. % Simulate some disturbance in the angular velocity.
13. % The magnitude of the deviation is in radians / second.
14. deviation = 100;
15. thetadot = deg2rad(2 * deviation * rand(3, 1) - deviation);
16. % Step through the simulation, updating the state.
17. for t = times
18.         % Take input from our controller.
19.         i = input(t);
20.         omega = thetadot2omega(thetadot, theta);
21.         % Compute linear and angular accelerations.
22.         a = acceleration(i, theta, xdot, m, g, k, kd);
23.        omegadot = angular_acceleration(i, omega, I, L, b, k);
24.         omega = omega + dt * omegadot;
25.         thetadot = omega2thetadot(omega, theta);
26.         theta = theta + dt * thetadot;
27.         xdot = xdot + dt * a;
28.         x = x + dt * xdot;
29. end
30. % Compute thrust given current inputs and thrust coefficient.
31. function T = thrust(inputs, k)
32.         % Inputs are values for wi^2
33.         T = [0; 0; k * sum(inputs)];
34. end
35. % Compute torques, given current inputs, length, drag coefficient, and thrust coefficient.
36. function tau = torques(inputs, L, b, k)
37.         % Inputs are values for wi
38.         tau = [L * k * (inputs(1) - inputs(3); L * k * (inputs(2) - inputs(4); b * (inputs(1) - inputs(2) + inputs(3) - inputs(4))];
39. end
40. function a = acceleration(inputs, angles, xdot, m, g, k, kd)
41.         gravity = [0; 0; -g];
42.         R = rotation(angles);
43.         T = R * thrust(inputs, k);
44.         Fd = -kd * xdot;
45.         a = gravity + 1 / m * T + Fd;
46. end
47. function omegadot = angular_acceleration(inputs, omega, I, L, b, k)
48.         tau = torques(inputs, L, b, k);
49.         omegaddot = inv(I) * (tau - cross(omega, I * omega));
50. end

• In real environment:

The noise in real environment is more complex to handle thus use kalman filter and various advanced PID techniques to make the quadcopter work. I will update this answer and give the real implementation of quadcopter on my github as soon as possible.

Now recall that I’ve said that noises in case of quadcopter are not correctable by PID , the PID is just modifying the error in an iterative fashion such that the quadcopter will remain in horizontal position if inputs given to controller unit are (0 , 0 ,0) = {Yaw, Pitch, Roll}.