簡(jiǎn)介面向嵌入式、機(jī)器人與無人機(jī)方向的姿態(tài)解算學(xué)習(xí)者這份MATLAB工程圍繞九軸傳感器三軸陀螺儀、加速度計(jì)與磁力計(jì)的數(shù)據(jù)融合展開覆蓋陀螺儀角速度、線性加速度和地磁觀測(cè)的校準(zhǔn)、濾波與姿態(tài)輸出適合已具備一定MATLAB基礎(chǔ)、希望把互補(bǔ)濾波、Mahony、擴(kuò)展卡爾曼濾波等方法落實(shí)到代碼中的讀者。壓縮包共12個(gè)文件以10個(gè).m腳本/函數(shù)為主體另有1個(gè).mat數(shù)據(jù)文件和1個(gè).asv自動(dòng)備份文件整體約555KB便于直接運(yùn)行、閱讀與二次修改。已有5071人學(xué)習(xí)說明其在慣導(dǎo)定位與姿態(tài)解算入門實(shí)踐中具有一定參考價(jià)值。資源按核心算法、傳感器校準(zhǔn)、數(shù)據(jù)處理與演示腳本組織包含低通/高通濾波、磁力計(jì)到加速度坐標(biāo)系轉(zhuǎn)換、歐拉角與四元數(shù)結(jié)果查看可幫助讀者理解從原始IMU數(shù)據(jù)到俯仰、偏航、翻滾角輸出的完整鏈路并據(jù)此搭建自己的實(shí)驗(yàn)流程。1. 九軸姿態(tài)解算的真實(shí)起點(diǎn)靜止十分鐘航向漂了三十度前陣子幫人看一臺(tái)田間小車的日志機(jī)器停在原地十分鐘串口打印的航向角從 32° 一路爬到 61°一動(dòng)起來 roll 和 pitch 又開始抽。傳感器沒壞問題出在從原始采樣到姿態(tài)四元數(shù)這條鏈路上標(biāo)定沒做干凈、磁力計(jì)沒做傾角補(bǔ)償、陀螺零偏被當(dāng)成真值一路積分。這份基于 MATLAB 的九軸傳感器姿態(tài)解算工程把這套鏈路完整攤開——加速度計(jì)、陀螺儀、磁力計(jì)三路數(shù)據(jù)怎么對(duì)齊零偏和橢球誤差怎么標(biāo)四元數(shù)怎么積分Mahony 和 Madgwick 兩種互補(bǔ)濾波怎么選姿態(tài)穩(wěn)定后怎么接慣導(dǎo)定位推速度和位置。適合正在調(diào) IMU 模塊、做云臺(tái)/小車/機(jī)械臂末端姿態(tài)或者準(zhǔn)備把 MATLAB 算法往嵌入式搬的開發(fā)者。2. 工程骨架與數(shù)據(jù)流從原始采樣到統(tǒng)一時(shí)間軸拿到一個(gè)姿態(tài)解算工程先別急著看濾波公式。九軸數(shù)據(jù)最容易翻車的地方不在算法本身而在“三路傳感器的時(shí)間戳對(duì)不上、坐標(biāo)軸定義各說各話”。這份工程把 IO、標(biāo)定、AHRS、INS、評(píng)估拆成五層每一層只做一件事改起來不會(huì)牽一發(fā)動(dòng)全身。2.1 目錄結(jié)構(gòu)與腳本職責(zé)工程按功能分層而不是按“第幾章第幾節(jié)”平鋪這樣二次開發(fā)時(shí)替換某一層不會(huì)影響其他模塊nine_axis_ahrs/ ├── data/ │ ├── calib_static.mat % 靜止采集用于陀螺零偏 │ ├── calib_six_face.mat % 六面采集用于加速度計(jì)標(biāo)定 │ ├── calib_mag_rotate.mat % 繞三軸慢速旋轉(zhuǎn)用于磁橢球擬合 │ └── run_dynamic.mat % 動(dòng)態(tài)跑車/搖擺數(shù)據(jù) ├── src/ │ ├── io/ │ │ └── load_dataset.m % 統(tǒng)一讀入、單位換算、時(shí)間對(duì)齊 │ ├── calib/ │ │ ├── gyro_bias_calib.m │ │ ├── acc_six_face_calib.m │ │ └── mag_ellipsoid_calib.m │ ├── ahrs/ │ │ ├── quat_multiply.m │ │ ├── quat_to_dcm.m │ │ ├── dcm_to_euler.m │ │ ├── mahony_update.m │ │ └── madgwick_update.m │ ├── ins/ │ │ ├── ins_mechanize.m % 比力變換、重力補(bǔ)償、積分 │ │ └── zupt_detect.m % 零速檢測(cè) │ └── eval/ │ ├── plot_attitude.m │ └── plot_trajectory.m ├── run_all.m % 一鍵跑通標(biāo)定→解算→畫圖 └── README.mddata里放的是采集好的.mat不依賴硬件就能復(fù)現(xiàn)calib下的三個(gè)腳本各自獨(dú)立可以單獨(dú)拿出來給別的項(xiàng)目用ahrs只負(fù)責(zé)“給定角速度和比力輸出四元數(shù)”不碰文件讀寫ins拿四元數(shù)做機(jī)械編排eval只畫圖不改數(shù)據(jù)。這種切法的好處是你要換 Madgwick 為 EKF只動(dòng)ahrs一個(gè)目錄run_all.m的調(diào)用方式不變。run_all.m我一般寫成顯式流水線而不是一堆函數(shù)互相調(diào)用方便打斷點(diǎn)% run_all.m % 1) 讀數(shù)據(jù) D load_dataset(data/run_dynamic.mat); % 2) 標(biāo)定參數(shù)首次運(yùn)行會(huì)從 calib_*.mat 里算出來并緩存 P load(data/calib_params.mat); % 3) 應(yīng)用標(biāo)定 gyro (D.gyro - P.gyro_bias) * diag(P.gyro_scale); acc (D.acc - P.acc_bias) * diag(P.acc_scale); mag (D.mag - P.mag_center) * P.mag_W; % 4) 逐采樣遞推 q [1;0;0;0]; for k 2:numel(D.t) dt D.t(k) - D.t(k-1); q mahony_update(q, gyro(k,:), acc(k,:), mag(k,:), dt, 0.5, 0.02); eul(k,:) dcm_to_euler(quat_to_dcm(q)); end % 5) 畫圖 plot_attitude(D.t, eul);這里的mahony_update入?yún)㈨樞蚴?q, gyro, acc, mag, dt, Kp, Ki)Kp取 0.5、Ki取 0.02 是常見起步值后面第 4 章會(huì)解釋這兩個(gè)數(shù)怎么調(diào)。dt不要用固定 1/fs用相鄰時(shí)間戳相減采集卡丟包時(shí)能自動(dòng)吸收誤差。2.2 數(shù)據(jù)格式與時(shí)間對(duì)齊九軸模塊常見的輸出是陀螺和加速度計(jì) 100~1000 Hz磁力計(jì) 10~100 Hz。如果直接把三路按行號(hào)一一對(duì)應(yīng)等于假設(shè)它們同時(shí)采樣磁力計(jì)那一行會(huì)整體滯后幾十毫秒航向在快速轉(zhuǎn)動(dòng)時(shí)明顯拖尾。工程里約定每個(gè)數(shù)據(jù)集至少包含以下字段字段維度單位說明t_gyroN×1s陀螺時(shí)間戳作為主時(shí)間軸gyroN×3rad/s機(jī)體坐標(biāo)系角速度accN×3m/s2比力含重力t_magM×1s磁力計(jì)時(shí)間戳magM×3μT機(jī)體坐標(biāo)系磁場(chǎng)fs標(biāo)量Hz標(biāo)稱采樣率僅作參考對(duì)齊用interp1把磁力計(jì)插到陀螺時(shí)間軸上注意外推標(biāo)志function D align_streams(D) % 以陀螺時(shí)間軸為主軸把磁力計(jì)線性插值過來 if isfield(D, t_mag) numel(D.t_mag) ~ numel(D.t_gyro) D.mag interp1(D.t_mag, D.mag, D.t_gyro, linear, extrap); end D.t D.t_gyro; % 去掉首尾各 1% 的數(shù)據(jù)避免插值邊界抖動(dòng) n numel(D.t); idx round(0.01*n)1 : n-round(0.01*n); D.t D.t(idx); D.gyro D.gyro(idx,:); D.acc D.acc(idx,:); D.mag D.mag(idx,:); endlinear對(duì)磁力計(jì)足夠因?yàn)榇艌?chǎng)變化本來就慢extrap是為了防止t_mag比t_gyro短一截時(shí)首尾出現(xiàn)NaN但外推出來的點(diǎn)不可信所以后面裁掉首尾 1%。如果磁力計(jì)采樣率低于 10 Hz線性插值的誤差會(huì)變大這時(shí)改用零階保持previous更穩(wěn)代價(jià)是航向更新有臺(tái)階。還有一個(gè)容易忽略的點(diǎn)單位。不同模塊陀螺輸出可能是deg/s磁力計(jì)可能是gauss讀入時(shí)必須統(tǒng)一到rad/s和μT否則Kp調(diào)到頭也壓不住漂移。load_dataset.m里我習(xí)慣加一段顯式檢查assert(max(abs(D.gyro(:))) 20, 陀螺單位疑似 deg/s請(qǐng)先轉(zhuǎn) rad/s); assert(mean(vecnorm(D.mag,2,2)) 5 mean(vecnorm(D.mag,2,2)) 200, ... 磁力計(jì)模值異常檢查單位是否為 uT);2.3 坐標(biāo)系約定與旋轉(zhuǎn)順序姿態(tài)解算里一半的“玄學(xué)”來自坐標(biāo)系沒對(duì)齊。工程統(tǒng)一采用航空常用的約定并在 README 第一行寫死符號(hào)含義約定b 系機(jī)體坐標(biāo)系x 向前y 向右z 向下FRDn 系導(dǎo)航坐標(biāo)系北-東-地NEDq姿態(tài)四元數(shù)[w;x;y;z]表示 b 系到 n 系的旋轉(zhuǎn)歐拉角ZYX 順序先繞 z 轉(zhuǎn) yaw再繞 y 轉(zhuǎn) pitch最后繞 x 轉(zhuǎn) rollg重力加速度9.81 m/s2NED 下沿 z在這個(gè)約定下靜止時(shí)加速度計(jì)輸出約[0 0 -9.81]比力方向向上與 z 軸反向磁力計(jì)在水平放置時(shí)北向分量為正。如果拿到的模塊是“前右下”之外的軸向比如 x 向右、y 向前必須在load_dataset里做一次軸交換而不是等到濾波里再補(bǔ)否則后面所有叉積的符號(hào)都是錯(cuò)的。歐拉角用 ZYX 順序反解時(shí)pitch在 ±90° 附近會(huì)出現(xiàn)萬向鎖atan2的分母趨近于零角度會(huì)跳變。這不是算法錯(cuò)誤是歐拉角表示本身的奇點(diǎn)第 5 章會(huì)講怎么在畫圖和輸出時(shí)繞開它。四元數(shù)全程參與運(yùn)算只在最后展示時(shí)轉(zhuǎn)歐拉角這樣積分過程不受奇點(diǎn)影響。3. 標(biāo)定與預(yù)處理零偏、六面校準(zhǔn)與磁橢球擬合很多人拿到模塊直接跑濾波看到曲線“大概跟著動(dòng)”就認(rèn)為成了結(jié)果航向一小時(shí)漂幾十度。九軸姿態(tài)解算的精度上限由標(biāo)定決定濾波只負(fù)責(zé)把剩下的誤差壓平。這一章三個(gè)標(biāo)定各管一段陀螺零偏管短期漂移加速度計(jì)六面管傾角基準(zhǔn)磁橢球管航向絕對(duì)參考。3.1 陀螺零偏與溫度漂移陀螺靜止時(shí)輸出不是零而是一個(gè)幾十到幾百deg/h的偏置直接積分就是航向漂移。最直接的做法是開機(jī)靜止采一段求均值function [bias, sigma] gyro_bias_calib(gyro, fs, sec) % gyro: N×3 rad/s % sec : 靜止采集時(shí)長(zhǎng)建議 30~60 s n min(round(sec*fs), size(gyro,1)); seg gyro(1:n, :); bias mean(seg, 1); sigma std(seg, 0, 1); % 用 3σ 剔除野點(diǎn)后重算避免開機(jī)瞬間的沖擊 keep all(abs(seg - bias) 3*sigma, 2); bias mean(seg(keep,:), 1); fprintf(gyro bias (rad/s): %.5f %.5f %.5f\n, bias); fprintf(gyro noise std : %.5f %.5f %.5f\n, sigma); endsec取 30 秒是經(jīng)驗(yàn)值太短均值不穩(wěn)太長(zhǎng)用戶等不及。sigma不是拿來好看的它是后面 Mahony 的Ki和 ZUPT 閾值的參考——噪聲標(biāo)準(zhǔn)差乘以采樣間隔就是單步積分的不確定度。如果sigma大于 0.01 rad/s先檢查是不是采集時(shí)桌子在抖或者模塊沒固定。零偏會(huì)隨溫度變冷機(jī)開機(jī)和跑十分鐘后的零偏能差 20% 以上。工程里沒有做完整溫補(bǔ)但留了一個(gè)線性模型接口% 若數(shù)據(jù)里帶溫度字段 D.temp攝氏度可以做一階擬合 p polyfit(D.temp(static_idx), gyro_static(:,1), 1); bias_x polyval(p, D.temp(k));我一般會(huì)在run_all.m里把冷機(jī)靜止段和熱機(jī)靜止段各存一份對(duì)比零偏差異超過 30% 就在 README 里注明“建議預(yù)熱 5 分鐘”。這是血淚經(jīng)驗(yàn)不寫這句話下一個(gè)用工程的人一定會(huì)在冷機(jī)狀態(tài)下調(diào)參數(shù)然后懷疑算法有問題。3.2 加速度計(jì)六面標(biāo)定加速度計(jì)的誤差模型是raw S * a_true bS是 3×3 的尺度加軸間耦合b是零偏。完整解需要十二面甚至更多姿態(tài)但工程里用六面法已經(jīng)能消掉主要誤差把模塊分別朝 x、-x、y、-y、z、-z 六個(gè)方向靜止放置每個(gè)方向采幾秒取均值。function [bias, scale] acc_six_face_calib(acc_faces, g) % acc_faces: 6×3行順序?yàn)?x,-x,y,-y,z,-z 的平均讀數(shù) % g: 當(dāng)?shù)刂亓铀俣?.81 bias zeros(3,1); scale zeros(3,1); for i 1:3 pos acc_faces(2*i-1, i); % 該軸指向下時(shí)的讀數(shù) neg acc_faces(2*i, i); % 該軸指向上時(shí)的讀數(shù) % 理想情況 pos g, neg -g scale(i) 2*g / (pos - neg); bias(i) (pos neg) / 2; end % 校正a_cal scale .* (raw - bias) end這里假設(shè)六個(gè)面放置時(shí)敏感軸嚴(yán)格對(duì)準(zhǔn)重力方向?qū)嶋H用手?jǐn)[會(huì)有幾度偏差scale會(huì)有 1%~2% 的誤差。對(duì)姿態(tài)解算來說夠用因?yàn)榧铀俣扔?jì)只用來修正 roll 和 pitch 的長(zhǎng)期漂移不參與高頻積分。bias的單位是 m/s2scale無量綱校正時(shí)逐軸相乘。如果模塊焊在板子上、板子又裝在結(jié)構(gòu)件里六個(gè)面很難擺正可以退而求其次只做零偏校正把模塊水平靜置取bias mean(acc) - [0 0 -g]。代價(jià)是傾角在±30° 以外會(huì)有幾度誤差適合對(duì)精度要求不高的場(chǎng)景。3.3 磁力計(jì)橢球擬合與硬磁/軟磁補(bǔ)償磁力計(jì)是航向的唯一絕對(duì)參考但它也是最容易被干擾的一路。硬磁干擾喇叭磁鐵、電機(jī)永磁體讓球心偏移軟磁干擾鐵質(zhì)結(jié)構(gòu)件讓球變成橢球。未標(biāo)定的磁力計(jì)轉(zhuǎn)一圈模值可能從 20 μT 變到 80 μT航向自然亂跳。工程里用九參數(shù)橢球擬合。把磁力計(jì)繞三個(gè)軸各轉(zhuǎn)幾圈采到的點(diǎn)滿足一般二次曲面function [center, W] mag_ellipsoid_calib(mag) % mag: N×3單位 μT需覆蓋各個(gè)朝向 mx mag(:,1); my mag(:,2); mz mag(:,3); % 設(shè)計(jì)矩陣Ax2By2Cz22Dxy2Exz2Fyz2Gx2Hy2Iz 1 D [mx.^2, my.^2, mz.^2, ... 2*mx.*my, 2*mx.*mz, 2*my.*mz, ... 2*mx, 2*my, 2*mz]; p D \ ones(size(mx)); % 還原二次型矩陣與一次項(xiàng) M [p(1) p(4) p(5); p(4) p(2) p(6); p(5) p(6) p(3)]; d p(7:9); center -M \ d; % 橢球球心即硬磁偏移 % 軟磁把 M 歸一化到單位球 [V, E] eig(M); s sqrt(diag(E) / min(diag(E))); W V * diag(1./s) * V; % 3×3 軟磁補(bǔ)償矩陣 endp是九維系數(shù)M是二次型矩陣d是一次項(xiàng)。完成配方后球心是-M\d也就是硬磁偏移W把橢球拉回球。應(yīng)用時(shí)mag_cal (mag - center) * W; % 檢查標(biāo)定效果模值標(biāo)準(zhǔn)差應(yīng)小于均值的 5% fprintf(mag norm mean%.2f, std%.2f\n, ... mean(vecnorm(mag_cal,2,2)), std(vecnorm(mag_cal,2,2)));參數(shù)說明D的列順序必須和p的物理含義一一對(duì)應(yīng)改順序會(huì)得到完全錯(cuò)誤的球心eig分解要求M正定如果采集點(diǎn)分布太集中比如只在一個(gè)平面轉(zhuǎn)M會(huì)接近奇異W里出現(xiàn)巨大數(shù)值。判斷方法是看cond(M)大于 1e6 就重新采數(shù)據(jù)讓模塊繞三個(gè)軸都轉(zhuǎn)到。標(biāo)定后的磁力計(jì)還要做傾角補(bǔ)償才能算航向。簡(jiǎn)單做法是把磁場(chǎng)旋到水平面function yaw mag_yaw(mag_b, roll, pitch) mx mag_b(1); my mag_b(2); mz mag_b(3); hx mx*cos(pitch) my*sin(roll)*sin(pitch) mz*cos(roll)*sin(pitch); hy my*cos(roll) - mz*sin(roll); yaw atan2(-hy, hx); % NED 下北向?yàn)?0順時(shí)針為正 end這段只在靜止或低動(dòng)態(tài)時(shí)可信動(dòng)態(tài)下更穩(wěn)的做法是把磁力計(jì)觀測(cè)直接送進(jìn) Mahony 或 Madgwick讓濾波器自己處理權(quán)重第 4 章會(huì)給出對(duì)應(yīng)代碼。4. 姿態(tài)解算與慣導(dǎo)定位四元數(shù)積分、Mahony、重力補(bǔ)償與 ZUPT標(biāo)定把系統(tǒng)性誤差壓下去之后剩下的就是濾波器的活。這一章從四元數(shù)微分方程講到 Mahony 和 Madgwick 兩種互補(bǔ)濾波再往下接慣導(dǎo)機(jī)械編排。代碼都按“能直接抄進(jìn)自己的工程”來寫參數(shù)含義和調(diào)法跟在每段后面。4.1 四元數(shù)更新與陀螺積分四元數(shù)q [w;x;y;z]表示 b 系到 n 系的旋轉(zhuǎn)微分方程是q_dot 0.5 * q ? [0; ω]其中ω是機(jī)體角速度。離散化后用一階歐拉積分再歸一化function q quat_integrate(q, omega, dt) % omega: 3×1 rad/s已扣除零偏并做濾波修正 wx omega(1); wy omega(2); wz omega(3); % 四元數(shù)乘法矩陣形式q_dot 0.5 * Omega * q Omega [0 -wx -wy -wz; wx 0 wz -wy; wy -wz 0 wx; wz wy -wx 0 ]; q q 0.5 * dt * (Omega * q); q q / norm(q); % 必須歸一化否則模值漂移會(huì)污染旋轉(zhuǎn)矩陣 enddt用相鄰時(shí)間戳之差不用固定值。一階積分在dt小于 10 ms 時(shí)誤差可忽略如果采樣率只有 50 Hz快速轉(zhuǎn)動(dòng)下會(huì)有明顯錐誤差這時(shí)換二階龍格庫(kù)塔或圓錐補(bǔ)償。歸一化那行不能省浮點(diǎn)累積幾十萬步之后模值會(huì)偏離 1旋轉(zhuǎn)矩陣就不再正交重力方向會(huì)慢慢歪掉。四元數(shù)轉(zhuǎn)旋轉(zhuǎn)矩陣用標(biāo)準(zhǔn)公式注意這里是 b 系到 n 系function R quat_to_dcm(q) q q / norm(q); w q(1); x q(2); y q(3); z q(4); R [1-2*(y^2z^2), 2*(x*y-w*z), 2*(x*zw*y); 2*(x*yw*z), 1-2*(x^2z^2), 2*(y*z-w*x); 2*(x*z-w*y), 2*(y*zw*x), 1-2*(x^2y^2)]; endR的第三行[2(xz-wy), 2(yzwx), 1-2(x2y2)]正好是 n 系重力單位向量[0;0;1]在 b 系的投影Mahony 的重力修正直接用它不用再單獨(dú)推公式。4.2 Mahony 互補(bǔ)濾波的 PI 修正Mahony 的思路是陀螺積分 trustworthy 在短時(shí)間加速度計(jì)和磁力計(jì) trustworthy 在長(zhǎng)時(shí)間。用加速度計(jì)觀測(cè)到的重力方向與四元數(shù)預(yù)測(cè)的重力方向做叉積得到誤差向量PI 控制后反饋到角速度上。function [q, b] mahony_update(q, b, gyro, acc, mag, dt, Kp, Ki) % q : 4×1 四元數(shù) % b : 3×1 積分反饋項(xiàng)陀螺零偏的在線估計(jì) % acc : 3×1 比力靜止時(shí)約 [0 0 -g] % mag : 3×1 磁場(chǎng)可為空 R quat_to_dcm(q); % 加速度計(jì)觀測(cè)取反歸一化得到 b 系下的“重力方向” a -acc(:) / norm(acc); % 四元數(shù)預(yù)測(cè)的重力方向R 的第三行 v R(3, :); e cross(a, v); % 磁力計(jì)修正把磁場(chǎng)轉(zhuǎn)到 n 系去掉垂直分量只保留水平北向 if nargin 5 ~isempty(mag) m mag(:) / norm(mag); h R * m; h(3) 0; if norm(h) 1e-6 h h / norm(h); v_m R * h; % 期望磁矢量在 b 系的方向 e e cross(m, v_m); end end % PI 反饋Kp 壓高頻誤差Ki 估零偏 if Ki 0 b b - Ki * e * dt; end omega gyro(:) Kp * e b; q quat_integrate(q, omega, dt); endKp決定收斂速度取 0.5~2.0Ki決定零偏跟蹤速度取 0.01~0.05。Kp太大姿態(tài)會(huì)跟著加速度計(jì)的振動(dòng)抖Ki太大航向會(huì)在磁干擾下緩慢跑偏。調(diào)參順序是先把Ki置零調(diào)Kp到靜態(tài)傾角穩(wěn)定、動(dòng)態(tài)不抖再加Ki讓長(zhǎng)時(shí)間靜止的航向不漂。b的初值給[0;0;0]它會(huì)在運(yùn)行中慢慢收斂到殘余零偏。注意b的符號(hào)b b - Ki*e*dt對(duì)應(yīng) Mahony 原文的b_dot -Ki*e如果寫成加號(hào)誤差會(huì)發(fā)散而不是收斂。4.3 Madgwick 梯度下降與 β 參數(shù)Madgwick 把姿態(tài)解算寫成優(yōu)化問題找一個(gè)四元數(shù)讓預(yù)測(cè)的重力方向和磁場(chǎng)方向與觀測(cè)的誤差最小用梯度下降迭代再和陀螺積分融合。它的計(jì)算量比 Mahony 略大但收斂更穩(wěn)尤其是在磁干擾間歇出現(xiàn)的場(chǎng)景。下面是加速度計(jì)部分的梯度磁力計(jì)部分結(jié)構(gòu)相同源碼里給了完整雅可比function q madgwick_update(q, gyro, acc, mag, dt, beta) q q / norm(q); a -acc(:) / norm(acc); % 與 Mahony 一致取反后 az≈1 ax a(1); ay a(2); az a(3); w q(1); x q(2); y q(3); z q(4); % 目標(biāo)函數(shù) f預(yù)測(cè)重力方向 - 觀測(cè)重力方向 f [2*(x*z - w*y) - ax; 2*(w*x y*z) - ay; 2*(0.5 - x^2 - y^2) - az]; % 雅可比 J df/dq J [-2*y, 2*z, -2*w, 2*x; 2*x, 2*w, 2*z, 2*y; 0, -4*x, -4*y, 0 ]; grad J * f; if norm(grad) 1e-9 grad grad / norm(grad); end % 陀螺積分項(xiàng) - β * 梯度修正項(xiàng) q_dot 0.5 * quat_multiply(q, [0; gyro(:)]) - beta * grad; q q q_dot * dt; q q / norm(q); endbeta是梯度下降步長(zhǎng)也是濾波器的“信任分配”旋鈕beta大更信加速度計(jì)收斂快但動(dòng)態(tài)噪聲大beta小更信陀螺平滑但傾角恢復(fù)慢。常用范圍 0.02~0.1對(duì)應(yīng)約 1~5 秒的收斂時(shí)間。如果采樣率是 100 Hzbeta取 0.05 左右比較穩(wěn)采樣率降到 50 Hzbeta要相應(yīng)減小否則每一步修正過沖。quat_multiply就是標(biāo)準(zhǔn)四元數(shù)乘法輸入兩個(gè) 4×1 列向量輸出也是 4×1function q quat_multiply(p, r) pwp(1); pxp(2); pyp(3); pzp(4); rwr(1); rxr(2); ryr(3); rzr(4); q [pw*rw - px*rx - py*ry - pz*rz; pw*rx px*rw py*rz - pz*ry; pw*ry - px*rz py*rw pz*rx; pw*rz px*ry - py*rx pz*rw]; end實(shí)際使用時(shí)不必自己手寫 Madgwick 的完整雅可比源碼里madgwick_update.m已經(jīng)把加速度計(jì)和磁力計(jì)兩部分拼好調(diào)用方式和 Mahony 一致方便在同一份數(shù)據(jù)上對(duì)比兩條曲線。4.4 姿態(tài)轉(zhuǎn)歐拉角與萬向鎖四元數(shù)轉(zhuǎn)歐拉角只在展示和評(píng)估時(shí)做ZYX 順序function eul dcm_to_euler(R) % 返回 [roll; pitch; yaw]單位 rad roll atan2(R(3,2), R(3,3)); pitch -asin(max(-1, min(1, R(3,1)))); % 限幅防 NaN yaw atan2(R(2,1), R(1,1)); eul [roll; pitch; yaw]; endasin外面套max/min是防止數(shù)值誤差讓R(3,1)略超 ±1 導(dǎo)致NaN。pitch接近 ±90° 時(shí)roll和yaw的分母同時(shí)趨近零兩個(gè)角會(huì)互相“搶”畫出來就是跳變。這不是解算錯(cuò)了是歐拉角在這個(gè)姿態(tài)下不唯一。評(píng)估時(shí)我一般把pitch限制在 ±85° 以內(nèi)做測(cè)試或者在圖上同時(shí)畫四元數(shù)分量避免被歐拉角跳變誤導(dǎo)。4.5 比力變換、重力補(bǔ)償與 ZUPT姿態(tài)穩(wěn)了之后慣導(dǎo)定位的鏈路是把機(jī)體比力轉(zhuǎn)到 n 系加回重力積分得速度再積分得位置function [vn, pn] ins_mechanize(q, acc, vn, pn, dt, g) % q : 當(dāng)前姿態(tài)四元數(shù) % acc: 3×1 比力單位 m/s2 % g : 9.81 R quat_to_dcm(q); f_n R * acc(:); % 比力轉(zhuǎn)到 n 系 a_n f_n [0; 0; g]; % NED 下重力沿 z vn vn a_n * dt; pn pn vn * dt; end靜止時(shí) 本文還有配套的精品資源點(diǎn)擊獲取