У меня есть акселерометр и гироскоп, объединенные в imu, и я хочу собирать данные (в течение небольшого промежутка времени, потому что шум влияет на результирующее положение).
Я пытаюсь смоделировать ситуацию, что мой IMU присоединяется к автомобилю (каркас кузова), движущемуся в системе координат XY (глобальная рама). Кузов автомобиля (IMU) может ускоряться в местном направлении X и Y. Вращая ось Z, автомобиль поворачивает в направлении X и Y в глобальной раме.
Я попытался построить первую позицию, когда угол поворота стал 45 °, и автомобиль разогнался на 9,81 м / с ^ 2 в направлении Y. Результат должен быть P (3.468 / 3.468), но это не так.
(Получил инструкцию "https://www.cl.cam.ac.uk/techreports/UCAM-CL-TR-696.pdf" (стр. 21-23))
I = eye(3);
C = [1;1;1];
vg = 0;
sg = 0;
wx = 0;
wy = 0;
wz = 45/180*pi; % calculation of angular velocity (yaw angle)
ax_b = 0; % acceleration in bodyframe on x-axis
ay_b = 1; % " " y-axis
az_b = 0; % " " z-axis
a_b = ([ax_b, ay_b, az_b]); % bodyframe accelerations
w_b = ([wx_b, wy_b,wz_b])'; % angular velocity
sigma = abs(wb*dt);
B = [0,-wz*dt,wy*dt;wz*dt,0,-wx*dt;-wy*dt,wx*dt,0];
C = C.*(I+(sin(sigma)/sigma).*B+((1-cos(sigma))/sigma.^2)*B.^2); % calculation of rotation matrix
ag = C*ab'; % global acceleration
vg = vg + dt.*(ag*9.81) % lobal velocity
sg = sg + dt*vg % global positon
Результат sg = 0, 9,8100, 0
результат должен быть:
sg = 3,468, 3,468, 0
Спасибо за помощь !!