%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % %惯性/里程计航位推算 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 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 177 天前 via Android 没用过 Octave 。之前公司没有 MATLAB license 我就用 numpy 代替了。 |
![]() | 2 l4mbda 177 天前 ![]() 矩阵和矢量运算较多,用 python 重新写一版试试 |
![]() | 3 Apol1oBelvedere 177 天前 Matlab 有更好的 JIT 编译优化,向量优化,库优化,多线程优化,内存优化,所以快,针对这几个方面去该代码或导入库来提升。 |
![]() | 4 icedx 177 天前 GNU 家的东西是这样的 |
5 yygymmq 176 天前 via iPhone 难得遇上做惯导的 支持一下 |