PID 제어기 알고리즘을 코드로 표현

표준 PID 제어기 알고리즘은 다음과 같습니다.

이 알고리즘을 코드로 간단하게 표현하자면 다음과 같습니다.

Error = Setpoint - Input;

PTerm = Kp * Error;

ITerm += Ki * Error * dt;

현재 오차(Error)는 설정(Setpoint) 값에서 현재 입력(Input) 값을 빼서 구합니다.
비례항(PTerm)은 비례항 이득값(Kp)에 현재 오차를 곱해서 구합니다.
적분항(ITerm)은 적분항 이득값(Ki)에 현재 오차와 주기(dt)를 곱해서 더해줍니다.

미분항(DTerm)을 구하기 위해서는 다음과 같이 구합니다.

dError = Error - prevError;

DTerm = Kd * (dError / dt);

오차 변화(dError)는 현재 오차(Error)에서 이전 오차(prevError)를 뺀 값입니다.
미분이라는 것이, 그래프의 순간의 기울기(오차 변화율)를 알고자 하는 것이기 때문에 짧은 순간의 오차 변화량을 순간 시간 변화량(주기)으로 나눠줍니다.(dError / dt)

마지막으로, 미분항(DTerm)은 미분항 이득값(Kd)에 오차 변화율을 곱해서 구합니다.

출력 값은 다음과 같습니다.

Output = PTerm + ITerm + DTerm;

참고 문헌 : 아두이노로 드론 만들고 직접 코딩하기