Skocz do zawartości

rmdiy

Użytkownicy
  • Zawartość

    3
  • Rejestracja

  • Ostatnio

Reputacja

0 Neutralna

O rmdiy

  • Ranga
    1/10

Informacje

  • Płeć
    Mężczyzna
  • Lokalizacja
    Rzeszów
  • Zawód
    Elektronik/Automatyk
  1. Otwieramy 2 eagle, group, copy group i w następnym oknie paste group ile razy chcemy.
  2. 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
  3. rmdiy

    PID-przejście z kąta na PWM

    Proporcjonalnie. Wraz ze wzrostem sygnału u(t) czyli wyjścia PID zwiększasz wypełnienie.
×