LSM9DS0의 Madgwick 센서 융합

Question

저는 Madmwick 센서 융합 알고리즘 from here을 LSM9DS0 센서 (가속도계, 자이로 스코프 및 자력계)에서 STM Cortex M3 마이크로 컨트롤러에 구현하려고합니다.

모든 센서의 원시 데이터가 양호한 것 같습니다.

내 문제는 z 축이 수평 또는 아래쪽 (즉, 롤 또는 피치 각도가 90도 이상)의 센서를 들고있을 때 필터의 쿼터니언이 실제로 불안정 해지고 무작위로 180도 뒤집습니다. 보다 정확하게, q0와 q3은 계속적으로 변화하는 부호를 가지므로 180도 회전합니다.

실제 센서 출력 대신 상수 값을 사용했지만이 동작이 계속되었습니다.

z 축이 다소 수직적 일 때 쿼터니온이 그럴듯한 것처럼 보입니다.

예를 들어 동영상에서 이와 같은 것을 보지 못했습니다.

나는 자력계 데이터를 무시하고 6DOF 버전의 필터를 사용하려고했지만 재앙이었다. 쿼터니언은 그냥 날아 다니며 통제 할 수없이 회전하고 있습니다.

갑자기 뒤집힌 값이 매우 낮아서 매우 불안정한 결과가 나오므로 베타 매개 변수를 매우 커야합니다 (~ 100). 보통 베타 버전이 0.5-0.05 정도이므로 이상하다고 생각합니다.

필터 업데이트주기는 1KHz입니다.

아무도 도와 줄 수 있습니까?

Answer 1

Here madgwick 필터의 코드에 실수가 있다고합니다!

s0= -_2q2*(2*(q1q3 - q0q2) - ax) + _2q1*(2*(q0q1 + q2q3) - ay) + -_4bz*q2*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx) + (-_4bx*q3+_4bz*q1)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my) + _4bx*q2*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz); 
s1= _2q3*(2*(q1q3 - q0q2) - ax) + _2q0*(2*(q0q1 + q2q3) - ay) + -4*q1*(2*(0.5 - q1q1 - q2q2) - az) + _4bz*q3*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx) + (_4bx*q2+_4bz*q0)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my) + (_4bx*q3-_8bz*q1)*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);    
s2= -_2q0*(2*(q1q3 - q0q2) - ax) +  _2q3*(2*(q0q1 + q2q3) - ay) + (-4*q2)*(2*(0.5 - q1q1 - q2q2) - az) + (-_8bx*q2-_4bz*q0)*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx)+(_4bx*q1+_4bz*q3)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my)+(_4bx*q0-_8bz*q2)*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz); 
s3= _2q1*(2*(q1q3 - q0q2) - ax) + _2q2*(2*(q0q1 + q2q3) - ay)+(-_8bx*q3+_4bz*q1)*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx)+(-_4bx*q0+_4bz*q2)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my)+(_4bx*q1)*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz); 

및 공식 사이트에서 해당 코드는 오래된 곧 대체 될 것이다 :

그라데이션 괜찮은 단계는 다음과 같아야합니다.

이렇게 수정하면 만족스러운 결과가 나타납니다.

다른 실수는 제대로 함수 프로토 타입을 읽지 못했습니다. 가속도계와 자이로 스코프 값이 서로 바뀌었던 코드의 약간 변경된 버전을 복사했습니다.