Robisz zmienne Ax, Ay, Az (w simie je już chyba masz).
Robisz zmienne Vx, Vy, Vz - prędkości.
Ustawiasz je na 0.
I teraz na przykład co 10ms sprawdzasz jakie są przyspieszenia.
Jak dodatnie do zwiększasz zmienne V, jak ujemne to zmniejszasz, jak równe 0 to nie ruszasz.
Oczywiście szybkość zwiększania V ma zależeć od A.
Np trzeba zrobić tak:
Co_10ms:
S = Ax * Ax_coefficient 'S - zmienna tymczasowa
Vx = Vx + S 'Ax_coefficient - ten współczynnik trzeba sobie dobrać
.... to samo dla y ....
.... to samo dla z ....
Teraz droga (albo nazwijmy to położenie).
W zasadzie tak samo.
Jak prędkość różna zero to nie ruszasz, jak dodatnia to zwiększasz, jak ujemna to zmniejszasz,
Co_10ms:
S = Vx * Vx_coefficient 'S - zmienna tymczasowa
Sx = Sx + S 'Vx_coefficient - ten współczynnik trzeba sobie dobrać
.... to samo dla y ....
.... to samo dla z ....
Tak to w teorii.
Ale teraz zmuszę cię zmartwić.
To nie zadziała.
Jakiekolwiek mały błąd pomiaru przyspieszenia (przyspieszenie różne od 0 przy braku ruchu) spowoduje że prędkość będzie ciągle zwiększana lub zmniejszana (powoli ale jednak). A to spowoduj z kolei zwiększanie lub zmniejszanie drogi.
I to lawinowo.
Trzeba co te 10ms na przykład wymnażać Vx * 0.999
Dzięki temu będzie powoli dążył do 0 co pozwoli zmniejszyć a nawet wyeliminować efekt uciekania wyników obliczeń do nieskończoności. Ale to spowoduje drastyczne zmniejszenie dokładności (o ile wcześniej jakakolwiek była) nie mówiąc już o nieliniowej deformacji wyników (większe częstotliwości będą przenoszone lepiej niż małe).
W praktyce tak się nie robi.
Nawet najlepsze akcelerometry, powiedzmy 16-to bitowe to mało.
Żeby wyliczyć prędkości i drogę trzeba dodać jeszcze inne czujniki (np. obrotu) i zastosować bardziej wyrafinowany algorytm. O wiele bardziej wyrafinowany. Wiec bo próbowałem i nic mi z tego nie wyszło.
Dopiero przy bardzo szybkich ruchach (powiedzmy > 10Hz) efekt "lawinowego uciekania" dostatecznie mocno zanikał i dało się uzyskać wyliczoną drogę cokolwiek podobną do faktycznego toru ruchu.
Acha. Jeszcze trzeba usunąć składową stałą z pomiarów przyspieszenia (filtrem powiedzmy na 0.01Hz).
I dochodzi jeszcze przyspieszenie ziemskie. Pochylenie czujnika przyspieszenia da jakieś niezerowe przyspieszenia w osiach co zaowocuje wynikiem w postaci ogromnej prędkości i ogromnej przebytej drogi.
Przy niepochylonym czujniku można sobie od wyniku w jednej osi (znaczy się osi Z) odjąć przyspieszenie ziemskie, ale gdy czujnik będzie pochylany to przyspieszenie ziemskie pojawi więc i w osiach X i Y a tego już tak łatwo sie nie odejmie.
A i to w teorii. W praktyce nawet nie pochylony czujnik coś tam zmierzy w osiach X i Y.
Dodano po 26 [minuty]: Zmienne oczywiście mają być typu Single.