Komentator Napisano Grudzień 30, 2014 Udostępnij Napisano Grudzień 30, 2014 Poprzednio przybliżyłem teorię związaną z filtrem Kalmana oraz na przykładzie pokazałem reakcję filtru na zmiany poszczególnych parametrów.Teraz zajmę się czymś bardziej praktycznym. Na podstawie danych pomiarowych z akcelerometru i żyroskopu postaram się zaprojektować filtr Kalmana dla układu śledzącego orientację robota w przestrzeni. UWAGA, to tylko wstęp! Dalsza część artykułu dostępna jest na blogu.Przeczytaj całość »Poniżej znajdują się komentarze powiązane z tym wpisem. Cytuj Link do komentarza Share on other sites More sharing options...
rmdiy Maj 25, 2015 Udostępnij Maj 25, 2015 Witam. Próbuję zrobić taki system : Jak widać, kąty Pitch, Roll są przeliczane z danych Acc, trafiają do KF razem z Gyro gdzie są estymowane. Estymowane wartości Pitch, Roll trafiają do "Tilt Compensation". Oto wzory modułu tilt : W praniu wygląda to tak: Może ktoś coś poradzić, na temat kąta Yaw. W zał. przesyłam plik do matlaba oraz dane w pliku tekstowym. clc; % get data from file % datatmp = importdata('data2.txt'); % data = datatmp.data; gyro_data = data(:,1:3); acc_data = data(:,4:6); mag_data=data(:,7:9); %data gyro_data = gyro_data*2000/32768; acc_data = acc_data*2/65535; mag_data = mag_data*4/65535; % time vector dt = 0.012; t = dt:dt:size(data, 1)*dt; %state space model A = [1 -dt; 0 1]; B = [dt; 0]; C = [1 0]; %measure and proces noise std_dev_v = 1; std_dev_w = 2; V = [std_dev_v*std_dev_v*dt 0; 0 std_dev_v*std_dev_v*dt]; W = std_dev_w*std_dev_w; % t(0) x0 = [0; 0]; P0 = [1 0; 0 1]; xpri = x0; Ppri = P0; xpost = x0; Ppost = P0; % Wektory wynikow Y = zeros(1, size(t,2)); Yf = Y; for i = 1:size(data, 1); %Obliczenie aktualnego kata na podstawie danych z akcelerometru % acc_angle = atan(acc_data(i,1)./acc_data(i,2))*180/pi; Pitch = atan(acc_data(i,1)./(acc_data(i,1).^2 + acc_data(i,3).^2))*180/pi; u = gyro_data(i,1); Y(i) = Pitch; if i == 1; % Pierwszy pomiar sluzy nam jako wartosc poczatkowa dla filtru xpost = [Pitch; 0]; else % aktualizacja czasu xpri = A*xpost + B*u; Ppri = A*Ppost*A' + V; % aktualizacja pomiarow eps = Y(i) - C*xpri; S = C*Ppri*C' + W; K = Ppri*C'*S^(-1); xpost = xpri + K*eps; Ppost = Ppri - K*S*K'; end %Zapis estymaty do wektora wynikow Yf(i) = xpost(1); end % Wektory wynikow Y1 = zeros(1, size(t,2)); Yf1 = Y1; for i = 1:size(data, 1); %Obliczenie aktualnego kata na podstawie danych z akcelerometru % acc_angle = atan(acc_data(i,1)./acc_data(i,2))*180/pi; Roll = atan(acc_data(i,2)./(acc_data(i,2).^2 + acc_data(i,3).^2))*180/pi; u = gyro_data(i,2); Y1(i) = Roll; if i == 1; % Pierwszy pomiar sluzy nam jako wartosc poczatkowa dla filtru xpost = [Roll; 0]; else % aktualizacja czasu xpri = A*xpost + B*u; Ppri = A*Ppost*A' + V; % aktualizacja pomiarow eps = Y1(i) - C*xpri; S = C*Ppri*C' + W; K = Ppri*C'*S^(-1); xpost = xpri + K*eps; Ppost = Ppri - K*S*K'; end %Zapis estymaty do wektora wynikow Yf1(i) = xpost(1); end XH = zeros(1, size(t,2)); YH = XH; Yaw = XH; %B = Yf(i) %L=Yf1(i) for i = 1:size(data, 1); XH(i) = mag_data(i,1)*cos(Yf(i)) + mag_data(i,2)*sin(Yf(i))*sin(Yf1(i)) + mag_data(i,3)*sin(Yf(i))*cos(Yf1(i)); YH(i) = mag_data(i,2)*cos(Yf1(i)) + mag_data(i,3)*sin(Yf1(i)); Yaw(i) = atan(-YH(i)/XH(i)) *180/pi; end subplot(3,1,1) plot(t, Y, 'b', t, Yf, 'r', 'LineWidth', 1.5) title('Pitch') xlabel('Czas') ylabel('Pitch [deg]') legend('ACC', 'KF out') subplot(3,1,2) plot(t, Y1, 'b', t, Yf1, 'r', 'LineWidth', 1.5) title('Roll') xlabel('Czas') ylabel('Roll [deg]') legend('Acc', 'KF out') subplot(3,1,3) plot(t, Yaw) title('Yaw') xlabel('Czas') ylabel('Yaw [deg]') dane.txt Cytuj Link do komentarza Share on other sites More sharing options...
simba92 Wrzesień 22, 2017 Udostępnij Wrzesień 22, 2017 Jaki byłby model stanowy dla filtru kalmana w celu wyliczenia kąta Yaw z wykorzystaniem magnetometru + żyroskopu ??? ktoś coś wie ??? Cytuj Link do komentarza Share on other sites More sharing options...
simba92 Wrzesień 22, 2018 Udostępnij Wrzesień 22, 2018 Cześć ! W artykule wzór na wyznaczenie wartości macierzy kowariancji Ppost jest następujący : Ppost = Ppri - K*S*K ale w literaturze przedmiotu wzór jest inny tzn. : Ppost = (I - K*C) * Ppri ;gdzie I jest macierzą jednostkową 2x2 i teraz mam pytanie do autora artykułu GandalF jak przekształcić wzór : Ppost = (I - KC)*Ppri do Ppost = Ppri - KSK? Proszę o pomoc w tej sprawie, bo próbowałem to przekształcić i cały czas mi się coś nie zgadza 😞 Cytuj Link do komentarza Share on other sites More sharing options...
Polecacz 101 Zarejestruj się lub zaloguj, aby ukryć tę reklamę. Zarejestruj się lub zaloguj, aby ukryć tę reklamę. Produkcja i montaż PCB - wybierz sprawdzone PCBWay! • Darmowe płytki dla studentów i projektów non-profit • Tylko 5$ za 10 prototypów PCB w 24 godziny • Usługa projektowania PCB na zlecenie • Montaż PCB od 30$ + bezpłatna dostawa i szablony • Darmowe narzędzie do podglądu plików Gerber Zobacz również » Film z fabryki PCBWay
Pomocna odpowiedź
Dołącz do dyskusji, napisz odpowiedź!
Jeśli masz już konto to zaloguj się teraz, aby opublikować wiadomość jako Ty. Możesz też napisać teraz i zarejestrować się później.
Uwaga: wgrywanie zdjęć i załączników dostępne jest po zalogowaniu!