자이로와 가속도센서를 이용한 각도추정 필터를 C로 구현하기

예전에

 자이로센서와 가속도센서를 이용한 각도추정 필터

를 소개한 적이 있는데요. 그 후

 각도추정필터의 계수를 최소자승법으로 구하는 방법

을 다시 소개했었습니다.  그런데 방문하신 몇몇 분들께서 C-code로 다시 표현해달라는 이야기를 하셨는데요. 사실 포스팅을 해야겠다고 생각만 하고 미뤄두고 있었네요. 근데 결과를 보시면 정말 간단하다는 사실에 아마 경악을 금치 못하실 겁니다.^^. 일단 다시 그 보정 필터의 개념도를 보도록 하지요.

위에 있네요. 저 그림에 주석을 좀 달고 다시 시작해보겠습니다.

먼저 

          Temp1

          intTemp1

          Temp2

          FilterAngle

순서로 구하게 됩니다. 그림상에 마지막부분의 FilterAngle은 단지 라디안(radian)을 degree로 표현을 다시 하는 것 뿐입니다. 결국 FilterAngle이라는 변수가 최종값으로 필터의 결과입니다. 

위에서 보상신호는 가속도센서에서 출력된 값을 이용해서 구한 각도입니다. 가속도센서의 출력이 아니라 거기서 구한 각도라는 것을 확인해주세요. 가속도센서에서 각도를 구하는 방법은 

-[프로젝트/HardwarePart] - 가속도센서를 이용한 각도 측정과 그 한계-

을 확인해주시구요. 그리고 자이로신호라고 되어있는 부분은 자이로신호를 그대로 넣어둔것이 아니라 자이로 신호에 스케일팩터를 반영

-[프로젝트/HardwarePart] - 자이로센서를 이용한 각도검출과 그 한계-

해서 각속도(rad/sec)의 결과입니다. 즉, 보상신호는 가속도센서에서 출력된 각도이고, 자이로신호는 자이로에서 출력된 각속도입니다. 

이제 그림의 Temp1을 계산하는 루틴입니다.

Temp1 = (-Pre_AccAngle) + Pre_FilterAngle;

위 코드에서 Pre_AccAngle은 이전의 가속도센서에서 구한 각도입니다. Pre_FilterAngle은 이전의 보정필터의 결과값입니다. 위 그림과 함께 보시면 됩니다.

다음은 intTemp1인데요. 이건 단순히 Temp1을 적분하면 되므로

IntTemp1 = IntTemp1 + (Temp1 * Ts);

위와같이 구현하면 끝입니다. 여기서 Ts는 sampling time으로 설정하고 계신 값을 그대로 사용해주면 됩니다. 혹시나 하고 이야기하는 거지만, 프로세서에서 구현하시는 분들은 타이머인터럽트를 사용하셔서 일정시간마다 계산하도록 하셔야합니다.

이제 Temp2인데요. 위 그림에서 보시다시피 Temp2는 Temp1에 Kp를 곱하고 intTemp1에 Ki를 곱하고, 그러고 난 다음 자이로의 출력각속도와 더해주어야합니다.

Temp2 = (-Kp * Temp1) + (-Ki * IntTemp1) + GyroAngularVel;

이렇게 되지요. GyroAngularVel이 자이로의 출력값입니다.

마지막으로 Temp2를 적분해서 FilterAngle을 구해야겠네요.

FilterAngle  = FilterAngle + (Temp2 * Ts);

위에 제시된 딱 4줄이 보정필터 전부입니다.^^. 이렇게 간단해서 제가 포스팅을 잘 못하고 있었습니다. 쓸말이 없어서 말이지요^^. 하지만, 제어블럭도를 잘 모르시는 경우 위 블럭만으로는 코드를 작성하는데 어려움이 있을거라는 생각이 들어서 이렇게 포스팅을 합니다.