%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%惯性/里程计航位推算
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
close all
%% 读取卫星数据
load('/data/bigfiles/GPSdata.mat') ;
%% 读取惯导数据
load( '/data/bigfiles/dataimu_200.mat');
Lati = GPSdata(1,1);
Longi = GPSdata(1,2);
Alti = GPSdata(1,3)-1.73;
gps=GPSdata(2:end,:);
%% 初始姿态
roll=-0.0031;
pitch=-0.0044;
yaw=-1.6753;
disp('***** 开始惯性/里程计航位推算 *****');
init_navigation_para;%初始化导航参数
%% 导航结果保存
Navi_result=zeros(1*3600,19);
%% 里程计 m 系和惯导 b 系的转换参数
detaTheta = -0.719512312424745*d2r;
detaPhi = -0.426290807070006*d2r;
% detaTheta = 0*d2r;
% detaPhi = 0*d2r;
for i=1:1*3600*200/2
tic;
%% 里程计 m 系和惯导 b 系的转换
Cbm= [ 1 -(detaPhi) (detaTheta)
(detaPhi) 1 0
-(detaTheta) 0 1] ;
Cmb = Cbm';
V_Odo_temp=(Odometer_vel(2*i-1,1)+Odometer_vel(2*i,1))/2;
V_Odo=Cbn*Cmb*[V_Odo_temp;0;0];
Vn=V_Odo(1);
Ve=V_Odo(2);
Vd=V_Odo(3);
%% 经纬高度更新,直接修位置
Odo_deta_S=V_Odo*2*sampt0;
Alti = Alti - Odo_deta_S(3);
Lati = Lati + Odo_deta_S(1)/(Rn+Alti);
Longi = Longi + Odo_deta_S(2)/((Re+Alti)*cos(Lati));
Rn = R0*(1-EE^2)/(1-EE^2*(sin(Lati))^2)^1.5; %更新子午圈半径
Re = R0/(1-EE^2*(sin(Lati))^2)^0.5; %更新横向曲率半径
%% 角增量和速度增量
ang_1 = d_angle(2*i-1,:)';
ang_2 = d_angle(2*i,:)';
Wien = [ WIE*cos(Lati) 0 -WIE*sin(Lati)]';
Wenn = [Ve/(Re+Alti) -Vn/(Rn+Alti) -Ve*tan(Lati)/(Re+Alti) ]';
Winn = Wien + Wenn;
Winb = Cnb * Winn;
%双子样等效转动矢量计算
ang_1 = ang_1 - Winb * sampt0;
ang_2 = ang_2 - Winb * sampt0;
angle = ang_1+ang_2;
TV = angle + (2.0/3.0)*cross(ang_1,ang_2);
%计算四元数
NS = TV' * TV ;
dM = [0 -TV(1) -TV(2) -TV(3)
TV(1) 0 TV(3) -TV(2)
TV(2) -TV(3) 0 TV(1)
TV(3) TV(2) -TV(1) 0];
QM = (1-NS/8.0+NS^2/384.0)*eye(4)+(0.5-NS/48.0)*dM;
q = QM* q;
q = q/norm(q);
%四元数→DCM
Cbn = [ q(1)^2+q(2)^2-q(3)^2-q(4)^2 2*(q(2)*q(3)-q(1)*q(4)) 2*(q(2)*q(4)+q(1)*q(3))
2*(q(2)*q(3)+q(1)*q(4)) q(1)^2-q(2)^2+q(3)^2-q(4)^2 2*(q(3)*q(4)-q(1)*q(2))
2*(q(2)*q(4)-q(1)*q(3)) 2*(q(3)*q(4)+q(1)*q(2)) q(1)^2-q(2)^2-q(3)^2+q(4)^2 ];
Cnb = Cbn';
if (mod(i,Imu_Frequency/2)==0)%在 1Hz 进行存储
Navi_result(gps_count,1) = gps_count;
Navi_result(gps_count,2)=Vn;
Navi_result(gps_count,3)=Ve;
Navi_result(gps_count,4)=Vd;
Navi_result(gps_count,5)=Lati*r2d;
Navi_result(gps_count,6)=Longi*r2d;
Navi_result(gps_count,7)=Alti ;
Navi_result(gps_count,8) =atan2(Cbn(3,2), Cbn(3,3))*r2d; % Roll
Navi_result(gps_count,9) =asin(-Cbn(3,1))*r2d; % Pitch
Navi_result(gps_count,10)=atan2(Cbn(2,1),Cbn(1,1))*r2d; % Yaw
gps_count = gps_count + 1;
end
end
filename='Navi_result_SINS_Odo_Navigaiton.mat';
save(strcat(filename), 'Navi_result');
这段代码,用: octave xxxx.m 要 130s ,matlab 只要 2s ,问题是这个代码里边的矩阵都太小了,用上了 openblas 和 parallel 包也是单核运行,不见丝毫提效。大的矩阵到是可以用上多线程了,但是这个场景每次迭代以来之前的结果,改都不好改。
octave:1> setenv('OPENBLAS_NUM_THREADS', '16');
A = rand(5000, 5000);
B = rand(5000, 5000);
tic;
C = A * B;
fprintf('大型矩阵乘法耗时 %f 秒\n', toc);
大型矩阵乘法耗时 129.283122 秒
octave:7> version -blas
ans = unknown or reference BLAS
# 终端运行
apt-get install libopenblas-dev
# 退出 octave 重新进入
octave:1> version -blas
ans = OpenBLAS (config: OpenBLAS 0.3.20 NO_LAPACKE DYNAMIC_ARCH NO_AFFINITY SkylakeX MAX_THREADS=64)
octave:2> setenv('OPENBLAS_NUM_THREADS', '16');
A = rand(5000, 5000);
B = rand(5000, 5000);
tic;
C = A * B;
fprintf('大型矩阵乘法耗时 %f 秒\n', toc);
大型矩阵乘法耗时 1.418475 秒
由于是工作需要用,没法用 matlab 的破解版,请教各路 V 友大神有好的办法加速么?
1
JeffGe 8 小时 8 分钟前 via Android
没用过 Octave 。之前公司没有 MATLAB license 我就用 numpy 代替了。
|
![]() |
2
l4mbda 8 小时 6 分钟前 ![]() 矩阵和矢量运算较多,用 python 重新写一版试试
|
![]() |
3
Apol1oBelvedere 7 小时 8 分钟前
Matlab 有更好的 JIT 编译优化,向量优化,库优化,多线程优化,内存优化,所以快,针对这几个方面去该代码或导入库来提升。
|
![]() |
4
icedx 5 小时 45 分钟前
GNU 家的东西是这样的
|