TIL (230626)

최근 드론의 수평제어를 위해 PX4의 gazebo에서 출력하는 IMU 센서 데이터를 기반으로 calibration을 수행하여 보정값을 다시 드론으로 전달하는 기능 구현이 필요하다 생각됐다. 결론부터 말하자면 삽질이었고 이미 PX4 펌웨어에서는 센서 데이터에 대해 확장칼만필터를 적용하여 제공하고 있어서 별도로 기능 구현을 할 필요가 없었다. 그래도 간단히나마 삽질 과정을 기록해본다.

 

찾다보니 IMU 센서 데이터를 보정하기 위해 칼만필터와 확장칼만필터가 사용되는 것을 알 수 있었다. 칼만필터는 선형시스템의 상태추정을 위해 사용되고 확장칼만필터는 비선형시스템의 상태추정을 위해 사용된다. 이러한 칼만필터와 확장칼만필터을 통해 드론의 자세추정과 수평제어에 사용할 수 있어 비행의 안정성을 가져올 수 있기 때문에 필수적이다. 칼만필터는 선형시스템의 실제상태와 추정상태 간의 차이를 나타내는 '오차공분산'을 최소화하는 알고리즘이다. 통계 기반 알고리즘이며 이전 추정상태가 다음 상태추정에 영향을 미치므로 마르코프 연쇄를 가정한 알고리즘이다. 이러한 칼만필터는 선형시스템일 경우 최적화된 상태 추정이 가능하다는 것이 수학적으로 증명되어 있다고 한다. 하지만 대부분의 경우 비선형시스템을 따르므로 모델링을 위해서는 확장칼만필터를 사용한다. 확장칼만필터는 비선형시스템을 나타내는 상태식을 매 순간 미분하여 선형시스템으로 근사한 다음 칼만필터를 적용하여 비선형시스템의 상태를 추정한다. 아래는 칼만필터의 알고리즘이다. 

 

칼만필터에 사용되는 주요 변수는 A, H, Q, R, P, K로 여섯 가지다. 이 중 네 가지(A, H, Q, R)는 이미 정해진 값이며 칼만이득인 K는 자연스럽게 계산되는 값이다. 실제로 구해야할 것은 P 밖에 없다. 이 P가 오차공분산이며 이를 계산하여 다음 상태추정에도 활용한다. 이 P를 최소화하는 것이 칼만필터 알고리즘의 목표다. 이러한 칼만필터를 활용해 IMU 센서 데이터를 보정하여 드론 수평제어 기능을 만들고 싶었다. 나이브했던 접근은 PX4 uORB topic 중 IMU 센서 관련 토픽을 구독해 칼만필터 알고리즘을 적용해 보정하고 보정값을 다시 토픽 발행을 통해 적용해주려 했다. 

 

 

PX4 uORB의 topic 중 /fmu/out/sensor_combined에서 IMU 센서값을 얻을 수 있었고 C++로 /fmu/out/sensor_combined 데이터를 구독하는 기능을 구현했다. 다만 칼만필터 알고리즘 적용이전 간단히 하드코딩된 값으로 설정해 발행해보았지만 적용되는 것 같지 않았다. 알고보니 /fmu/in/sensor_combined가 있을 것이고 여기로 보정값을 전달하면 수평제어가 되지 않을까 했었는데 /fmu/in/sensor_combined 토픽 자체가 없었던 것이다. 왜 없을까 생각해보았을 때 어쩌면 안정성을 추구하는 PX4의 철학때문이지 않을까 싶다. 잘은 알지 못하겠다. 아무튼 이렇게 IMU 센서 관련 정보를 직접적으로 calibration할 수 없다면 어떻게 이뤄질까하며 찾다보니 /fmu/out/sensor_combined는 이미 확장칼만필터가 적용된 센서값임을 알 수 있었다. PX4에는 이러한 확장칼만필터와 같은 기능을 활성화할지 비활성화할지를 결정하는 여러 개의 'parameter'가 있다. 이러한 paramter는 PX4의 펌웨어에 접근하거나 QGroundController 내부에서 변경이 가능하다. (https://docs.px4.io/v1.11/en/advanced_config/parameter_reference.html)

 

위 parameter reference에서 보다보면 확장칼만필터(EKF) 관련 parameter가 수 십여 개가 있는 것을 확인할 수 있다. EKF2_*로 시작하는 parameter들이다. 이러한 parameter들을 포함하여 PX4에서 사용하는 여러 paramter들은 PX4 셸에서 param show 명령을 통해서도 그 목록을 확인할 수 있다. 또 param show -c 옵션을 통해 default 값에서 바뀐 모든 매개변수를 확인할 수도 있다.

 

 

또 이런 parameter들은 PX4가 실행할 때 start script인 /PX4-Autopilot/ROMFS/px4fmu_common/init.d-posix/rcS 파일에서도 설정할 수 있음을 확인할 수 있었다.

 

 

또 PX4 셸에 구현되어 있는 uORB 앱 중에서 확장칼만필터를 보완한 ekf2를 확인할 수 있었다. ekf2 start를 통해 실행시킬 수도 있는 것 같은데 에러가 발생한다. 잘 모르겠다.

 

 

참고로 이 확장칼만필터인 ekf2는 PX4-Autopilot/src/modules/ekf2에서 구현체를 확인할 수 있다. 워낙 PX4 관련 레퍼런스가 적고 여기저기 찾으면서 부분을 더듬고 있다보니 작성한 글에도 오류가 있을진 모르겠다. PX4에 대해 익숙해진다면 고생하는 초심자들을 위해 관련 가이드 서적을 만들고 싶단 생각이 든다. 다음엔 어떤 기능을 만들어야 할까.