如果获得了当前时刻的航向角和角速度,需要外推出下一时刻的航向角,这里尝试六种方法计算。
1. 利用角速度和时间直接累积求和得到航向角。
2. 将航向角和角速度都转换到旋转矩阵,通过矩阵相乘得到航向角。
3. 将角速度计算反对称矩阵,再通过指数变换转为李群,在李群上相乘得到航向角。
4. 将角速度计算反对称矩阵,直接在李代数空间相加求和,最后转为李群相乘得到航向角。
5. 将航向角转为四元数,利用一阶航向角积分的毕卡算法得到航向角。
6. 将航向角转为四元数,利用四阶龙科库塔法积分得到航向角。
matlab代码如下:
clear all;close all;clc; dt = 0.02; %准备数据 t = 0:dt:10; num = length(t); yawrate1 = rand(int32(num/2)-1,1)*0.5; yawrate2 = rand(int32(num/2),1)*0.5-0.3; yawrate=[yawrate1;yawrate2]; re1=zeros(num,1); re2=zeros(num,1); re3=zeros(num,1); re4=zeros(num,1); re5=zeros(num,1); re6=zeros(num,1); startyaw = 0; %角速度直接求和求旋转角(欧拉角视角) yaw = startyaw; for i=1:num yaw = yaw + dt*yawrate(i); re1(i) = yaw; end %角速度通过旋转矩阵相乘求旋转角(旋转矩阵视角) R = angle2dcm(startyaw,0,0); for i=1:num R = R*angle2dcm(dt*yawrate(i),0,0); [yaw,~,~] = dcm2angle(R); re2(i) = yaw; end %角速度通过反对称矩阵的指数矩阵相乘求旋转角(李群视角) R = angle2dcm(startyaw,0,0); for i=1:num H = hat([0 0 dt*yawrate(i)])'; R = R*expm(H); [yaw,~,~] = dcm2angle(R); re3(i) = yaw; end %角速度通过反对称矩阵相加求旋转角(李代数视角) R = angle2dcm(startyaw,0,0); newH = zeros(3,3); for i=1:num H = hat([0 0 dt*yawrate(i)])'; newH = newH + H; newR = R*expm(newH); [yaw,~,~] = dcm2angle(newR); re4(i) = yaw; end %角速度通过四元数求旋转角(四元数视角-毕卡算法) q = angle2quat(startyaw,0,0)'; for i=1:num wx = 0; wy = 0; wz = yawrate(i); q_diff = Quaternion_Diff([wx wy wz],q); q = q + dt*q_diff; [yaw,~,~] = quat2angle(q'); re5(i) = yaw; end %角速度通过四元数求旋转角(四元数视角-四阶龙格库塔积分) q = angle2quat(startyaw,0,0)'; for i=1:num wx = 0; wy = 0; wz = yawrate(i); q = Quaternion_RungeKutta4(q,[wx wy wz],dt); [yaw,~,~] = quat2angle(q'); re6(i) = yaw; end figure; plot(wrapToPi(re1),'r-o'); hold on; plot(re2,'r-*'); plot(re3,'b-*'); plot(re4,'g-.'); plot(re5,'b-o'); plot(re6,'g-o'); function H = hat(w) H = [0 -w(3) w(2); w(3) 0 -w(1); -w(2) w(1) 0]; end function q = Quaternion_RungeKutta4( q0,gyro,T) K1 = Quaternion_Diff( gyro,q0); q1 = q0 + T/2*K1; K2 = Quaternion_Diff(gyro,q1); q2 = q0 + T/2*K2; K3 = Quaternion_Diff(gyro,q2); q3 = q0 + T*K3; K4 = Quaternion_Diff(gyro,q3); q = q0 + T/6*(K1+2*K2+2*K3+K4); end function q_diff = Quaternion_Diff( gyro,q) A = [ 0, -gyro(1)/2, -gyro(2)/2, -gyro(3)/2; gyro(1)/2, 0, gyro(3)/2, -gyro(2)/2; gyro(2)/2, -gyro(3)/2, 0, gyro(1)/2; gyro(3)/2, gyro(2)/2, -gyro(1)/2, 0]; q_diff = A*q; end
结果如下:
曲线基本重叠了,放大可以看出还是有点区别的。
四元数的两种方法计算似乎都有误差。
不过真实情况是计算方法带来的误差很可能都被噪声淹没了。
标签:end,gyro,yaw,练习,航向,num,matlab,角速度 From: https://www.cnblogs.com/tiandsp/p/16908399.html