欧美成人午夜精品久久久,国产?V天堂一区二区三区,欧美精品va在线观看,亚洲一区二区三区免费在线观看,av无码精品一区二区久久,欧美性爱视频不卡一区三区,欧美乱人伦视频在线观看,国产一级牲交高潮

ARTICLE DETAIL

資訊詳情

深耕商務(wù)建站與企業(yè)官網(wǎng)運(yùn)營的一線實(shí)戰(zhàn)洞察。

MATLAB九軸IMU姿態(tài)解算:標(biāo)定、四元數(shù)與互補(bǔ)濾波

MATLAB九軸IMU姿態(tài)解算:標(biāo)定、四元數(shù)與互補(bǔ)濾波 簡介面向嵌入式、機(jī)器人與無人機(jī)方向的姿態(tài)解算學(xué)習(xí)者這份MATLAB工程圍繞九軸傳感器三軸陀螺儀、加速度計(jì)與磁力計(jì)的數(shù)據(jù)融合展開覆蓋陀螺儀角速度、線性加速度和地磁觀測的校準(zhǔn)、濾波與姿態(tài)輸出適合已具備一定MATLAB基礎(chǔ)、希望把互補(bǔ)濾波、Mahony、擴(kuò)展卡爾曼濾波等方法落實(shí)到代碼中的讀者。壓縮包共12個(gè)文件以10個(gè).m腳本/函數(shù)為主體另有1個(gè).mat數(shù)據(jù)文件和1個(gè).asv自動備份文件整體約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)靜止十分鐘航向漂了三十度前陣子幫人看一臺田間小車的日志機(jī)器停在原地十分鐘串口打印的航向角從 32° 一路爬到 61°一動起來 roll 和 pitch 又開始抽。傳感器沒壞問題出在從原始采樣到姿態(tài)四元數(shù)這條鏈路上標(biāo)定沒做干凈、磁力計(jì)沒做傾角補(bǔ)償、陀螺零偏被當(dāng)成真值一路積分。這份基于 MATLAB 的九軸傳感器姿態(tài)解算工程把這套鏈路完整攤開——加速度計(jì)、陀螺儀、磁力計(jì)三路數(shù)據(jù)怎么對齊零偏和橢球誤差怎么標(biāo)四元數(shù)怎么積分Mahony 和 Madgwick 兩種互補(bǔ)濾波怎么選姿態(tài)穩(wěn)定后怎么接慣導(dǎo)定位推速度和位置。適合正在調(diào) IMU 模塊、做云臺/小車/機(jī)械臂末端姿態(tài)或者準(zhǔn)備把 MATLAB 算法往嵌入式搬的開發(fā)者。2. 工程骨架與數(shù)據(jù)流從原始采樣到統(tǒng)一時(shí)間軸拿到一個(gè)姿態(tài)解算工程先別急著看濾波公式。九軸數(shù)據(jù)最容易翻車的地方不在算法本身而在“三路傳感器的時(shí)間戳對不上、坐標(biāo)軸定義各說各話”。這份工程把 IO、標(biāo)定、AHRS、INS、評估拆成五層每一層只做一件事改起來不會牽一發(fā)動全身。2.1 目錄結(jié)構(gòu)與腳本職責(zé)工程按功能分層而不是按“第幾章第幾節(jié)”平鋪這樣二次開發(fā)時(shí)替換某一層不會影響其他模塊nine_axis_ahrs/ ├── data/ │ ├── calib_static.mat % 靜止采集用于陀螺零偏 │ ├── calib_six_face.mat % 六面采集用于加速度計(jì)標(biāo)定 │ ├── calib_mag_rotate.mat % 繞三軸慢速旋轉(zhuǎn)用于磁橢球擬合 │ └── run_dynamic.mat % 動態(tài)跑車/搖擺數(shù)據(jù) ├── src/ │ ├── io/ │ │ └── load_dataset.m % 統(tǒng)一讀入、單位換算、時(shí)間對齊 │ ├── 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 % 零速檢測 │ └── 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只動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)行會從 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 章會解釋這兩個(gè)數(shù)怎么調(diào)。dt不要用固定 1/fs用相鄰時(shí)間戳相減采集卡丟包時(shí)能自動吸收誤差。2.2 數(shù)據(jù)格式與時(shí)間對齊九軸模塊常見的輸出是陀螺和加速度計(jì) 100~1000 Hz磁力計(jì) 10~100 Hz。如果直接把三路按行號一一對應(yīng)等于假設(shè)它們同時(shí)采樣磁力計(jì)那一行會整體滯后幾十毫秒航向在快速轉(zhuǎn)動時(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)系磁場fs標(biāo)量Hz標(biāo)稱采樣率僅作參考對齊用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ù)避免插值邊界抖動 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對磁力計(jì)足夠因?yàn)榇艌鲎兓緛砭吐齟xtrap是為了防止t_mag比t_gyro短一截時(shí)首尾出現(xiàn)NaN但外推出來的點(diǎn)不可信所以后面裁掉首尾 1%。如果磁力計(jì)采樣率低于 10 Hz線性插值的誤差會變大這時(shí)改用零階保持previous更穩(wěn)代價(jià)是航向更新有臺階。還有一個(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請先轉(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)系沒對齊。工程統(tǒng)一采用航空常用的約定并在 README 第一行寫死符號含義約定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ǔ)否則后面所有叉積的符號都是錯(cuò)的。歐拉角用 ZYX 順序反解時(shí)pitch在 ±90° 附近會出現(xiàn)萬向鎖atan2的分母趨近于零角度會跳變。這不是算法錯(cuò)誤是歐拉角表示本身的奇點(diǎn)第 5 章會講怎么在畫圖和輸出時(shí)繞開它。四元數(shù)全程參與運(yùn)算只在最后展示時(shí)轉(zhuǎn)歐拉角這樣積分過程不受奇點(diǎn)影響。3. 標(biāo)定與預(yù)處理零偏、六面校準(zhǔn)與磁橢球擬合很多人拿到模塊直接跑濾波看到曲線“大概跟著動”就認(rèn)為成了結(jié)果航向一小時(shí)漂幾十度。九軸姿態(tài)解算的精度上限由標(biāo)定決定濾波只負(fù)責(zé)把剩下的誤差壓平。這一章三個(gè)標(biāo)定各管一段陀螺零偏管短期漂移加速度計(jì)六面管傾角基準(zhǔn)磁橢球管航向絕對參考。3.1 陀螺零偏與溫度漂移陀螺靜止時(shí)輸出不是零而是一個(gè)幾十到幾百deg/h的偏置直接積分就是航向漂移。最直接的做法是開機(jī)靜止采一段求均值function [bias, sigma] gyro_bias_calib(gyro, fs, sec) % gyro: N×3 rad/s % sec : 靜止采集時(shí)長建議 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)太長用戶等不及。sigma不是拿來好看的它是后面 Mahony 的Ki和 ZUPT 閾值的參考——噪聲標(biāo)準(zhǔn)差乘以采樣間隔就是單步積分的不確定度。如果sigma大于 0.01 rad/s先檢查是不是采集時(shí)桌子在抖或者模塊沒固定。零偏會隨溫度變冷機(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));我一般會在run_all.m里把冷機(jī)靜止段和熱機(jī)靜止段各存一份對比零偏差異超過 30% 就在 README 里注明“建議預(yù)熱 5 分鐘”。這是血淚經(jīng)驗(yàn)不寫這句話下一個(gè)用工程的人一定會在冷機(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)格對準(zhǔn)重力方向?qū)嶋H用手?jǐn)[會有幾度偏差scale會有 1%~2% 的誤差。對姿態(tài)解算來說夠用因?yàn)榧铀俣扔?jì)只用來修正 roll 和 pitch 的長期漂移不參與高頻積分。bias的單位是 m/s2scale無量綱校正時(shí)逐軸相乘。如果模塊焊在板子上、板子又裝在結(jié)構(gòu)件里六個(gè)面很難擺正可以退而求其次只做零偏校正把模塊水平靜置取bias mean(acc) - [0 0 -g]。代價(jià)是傾角在±30° 以外會有幾度誤差適合對精度要求不高的場景。3.3 磁力計(jì)橢球擬合與硬磁/軟磁補(bǔ)償磁力計(jì)是航向的唯一絕對參考但它也是最容易被干擾的一路。硬磁干擾喇叭磁鐵、電機(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的物理含義一一對應(yīng)改順序會得到完全錯(cuò)誤的球心eig分解要求M正定如果采集點(diǎn)分布太集中比如只在一個(gè)平面轉(zhuǎn)M會接近奇異W里出現(xiàn)巨大數(shù)值。判斷方法是看cond(M)大于 1e6 就重新采數(shù)據(jù)讓模塊繞三個(gè)軸都轉(zhuǎn)到。標(biāo)定后的磁力計(jì)還要做傾角補(bǔ)償才能算航向。簡單做法是把磁場旋到水平面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這段只在靜止或低動態(tài)時(shí)可信動態(tài)下更穩(wěn)的做法是把磁力計(jì)觀測直接送進(jìn) Mahony 或 Madgwick讓濾波器自己處理權(quán)重第 4 章會給出對應(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); % 必須歸一化否則模值漂移會污染旋轉(zhuǎn)矩陣 enddt用相鄰時(shí)間戳之差不用固定值。一階積分在dt小于 10 ms 時(shí)誤差可忽略如果采樣率只有 50 Hz快速轉(zhuǎn)動下會有明顯錐誤差這時(shí)換二階龍格庫塔或圓錐補(bǔ)償。歸一化那行不能省浮點(diǎn)累積幾十萬步之后模值會偏離 1旋轉(zhuǎn)矩陣就不再正交重力方向會慢慢歪掉。四元數(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 在長時(shí)間。用加速度計(jì)觀測到的重力方向與四元數(shù)預(yù)測的重力方向做叉積得到誤差向量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 磁場可為空 R quat_to_dcm(q); % 加速度計(jì)觀測取反歸一化得到 b 系下的“重力方向” a -acc(:) / norm(acc); % 四元數(shù)預(yù)測的重力方向R 的第三行 v R(3, :); e cross(a, v); % 磁力計(jì)修正把磁場轉(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)會跟著加速度計(jì)的振動抖Ki太大航向會在磁干擾下緩慢跑偏。調(diào)參順序是先把Ki置零調(diào)Kp到靜態(tài)傾角穩(wěn)定、動態(tài)不抖再加Ki讓長時(shí)間靜止的航向不漂。b的初值給[0;0;0]它會在運(yùn)行中慢慢收斂到殘余零偏。注意b的符號b b - Ki*e*dt對應(yīng) Mahony 原文的b_dot -Ki*e如果寫成加號誤差會發(fā)散而不是收斂。4.3 Madgwick 梯度下降與 β 參數(shù)Madgwick 把姿態(tài)解算寫成優(yōu)化問題找一個(gè)四元數(shù)讓預(yù)測的重力方向和磁場方向與觀測的誤差最小用梯度下降迭代再和陀螺積分融合。它的計(jì)算量比 Mahony 略大但收斂更穩(wěn)尤其是在磁干擾間歇出現(xiàn)的場景。下面是加速度計(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ù)測重力方向 - 觀測重力方向 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是梯度下降步長也是濾波器的“信任分配”旋鈕beta大更信加速度計(jì)收斂快但動態(tài)噪聲大beta小更信陀螺平滑但傾角恢復(fù)慢。常用范圍 0.02~0.1對應(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ù)上對比兩條曲線。4.4 姿態(tài)轉(zhuǎn)歐拉角與萬向鎖四元數(shù)轉(zhuǎn)歐拉角只在展示和評估時(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è)角會互相“搶”畫出來就是跳變。這不是解算錯(cuò)了是歐拉角在這個(gè)姿態(tài)下不唯一。評估時(shí)我一般把pitch限制在 ±85° 以內(nèi)做測試或者在圖上同時(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)擊獲取
返回列表
PREV
查看更多資訊
NEXT
返回資訊列表
婷婷五月综合视频免费播放| 婷婷激情伍月网| 热久精品| 国产精品岛国片在线观看免费| 久久视屏这里只有久久| 亚洲精品永久久久久久| 成人无码髙潮喷水A片| 开心久久爱五月天| 六月丁香综合| 欧美一级色| 激情五月黄色| 伊人国产婷婷五月天| 五月婷婷丁香综合,亚洲天堂| 精品久久久久久久久久久久人妻| 天天舔天天摸天天透| 久久99精品久久久久久噜噜| 综合 激情 婷婷| 操逼福利视频| 人人97碰| 久久婷婷大香蕉| 五月丁香六月婷婷,婷| 五月草影视| 久久免片| 性色婷婷| 99热99网| www激情| 日本久久久97| 五月婷婷伊人在线| 婷婷综合五月| 91丨九色丨国产打屁股| 久99久在线| 激情五月综合网| 综合啪啪| 五月婷婷|欧美| 五月婷婷自拍| 无码一级片| 激情丁香网| 久婷婷久草| 五月激情小说网| 伊人在线大香蕉网| 成人五月丁香社区| 99热国品免费| 久久久精品色| 丁香五月激情六月欧亚激情综合导航| 夜夜躁婷婷AV| 国产成人VA| 日木WWW视频| 五月开心婷婷| 9一精品视频观看| 99热无码首页| www.五月天色色色| 99色在线| A久久| 综合99久久天天综合| 妻久久人久久| 香蕉综合网| 亚洲视频在线观看| 免费观看的婷婷五月视频在线| 欧美三级A做爰在线观看| 五月激情婷婷女| 丁香五月手机在线| 色狠狠色| 天天爽天天操| 婷婷色播色五月五色五月天色妇| 女人高潮内射99精品| 99热99re6国产在线播放| 六月婷婷久久| 色色五月激情| 久热99| 天天摸天天日天天舔| 久久全色| 大地9中文在线观看免费高清| 婷婷六月色丁香视频在线观看| 亚洲9久久精品| 激情av在线| 日本黄色一级| 亚洲综合成人网| 婷婷五月天开心网| 四川女人毛多水多A片| 婷婷伊人激情婷婷| 久久新| 婷婷五月六月| 丁香五月影院| 99热主页日本| 婷婷五月天在线综合| 性色婷婷| 婷婷伊人五月丁香天堂网| www.久久99| 婷婷综合网在线| 91干| 国产色网站| 热日韩欧美| 日韩亚洲视频| 欧美性丁香色色五月天干干| 五月激激网w'w'w| 综合婷婷| 国产精品电影| 日韩AV片| 狠狠丁香| 91久热| 99男人的天堂| 久久精品99久久久久久久久| 九月婷婷综合网| 亚洲中文AV网站| 极品人妻VIDEOSSS人妻| 天天做天天爽| 久久资源网五月婷| 激情五月婷婷六月丁香| www.99热| 五月丁香综合激情网| 五月丁查人人| z色五月播播久久| 就去涩涩丁香五月天| 六月婷婷色综合| 99爱视频在线观看这里只有精品| 天堂A∨在线| av人人干| 五月天色综合| 天天骑天天操| 少妇高潮呻吟A片免费看软件| 天天揷综合网| 久9久视频精品| 91猫咪国产在线播放| 精品A√| 五月天婷婷色在线视频免费观看| 亚洲九九夜夜| 天天综合干| 九九热这里只有精品31| 91大屁股精品| 高清视频一区| 亚洲综合在线视频| www.色婷婷| 久久无码成人| 一区二区成人电影免费播放| 九月丁香婷婷基地| 五月婷婷黄色| 国产视频福利| 久久黄色免费视频| 婷婷六月天天| 99激情视频| 五月婷在线观看| 日韩无码色色| 色婷婷另类| 3p久久| 色色色777| 成人丁香五月婷| 嫩BBB槡BBBB搡BBBB视频| 五月丁香自拍| 五月天综合网| 五月天综合久久丁香91| 国产婷婷色五月| 五月天自拍网| 停婷丁五月在线| 在线观看免费观看在线9久| 亚洲成人电影在线免费观看| 欧美大香蕉视频| 深爱五月天 开心网| 色五月综合| 99久久综合狠狠综合久久| 91丨九色丨国产打屁股| 六月综合婷婷开心伊人| 久久五月婷天天干| 婷婷97狠狠干| 自拍盗摄 另类| 91超级碰| 久久精品63| 日韩婷婷五月天| 久操福利| 亚洲天堂有码| 天天色综合综合| 九九视频在线观看视频6| 五月丁香婷色| 伊人日日干| 亚洲婷婷五月| 亚洲狠狠色丁香婷婷综合久久| 六月激情丁香一道本7777| 五月丁香综合网色欲| 色久五月天| 国产乱人偷精品人妻A片| 一级七香蕉| 五月天另类激情在线| av成人在线播放| 能看的AV| 五月丁香六月香综合激情| 色欲天天综合网| 思思99热| 婷婷开心青青草| 欧洲MV日韩MV国产| 天天舔日日肏夜夜爽| BBWCUCKOLD精品熟妇| 婷婷爱在线观看| 99热这里只有精品8| 国内精品免费一区二区2009| 26uuu| 97碰 在线视频观看| 天天日天天操天天干| www激情| 丁香六月婷婷社区| 精品人妻一区二区三区四区不卡在| 欧美成人精品A片免费一区99| 婷婷伊人激情婷婷| 婷婷久久综合| 五月婷婷丁香狠狠撸久久| 欧美三级大片AA在线看| 99视频这里有精品| 久热中文字幕| 91久久99久久91熟女精品| 在线网黄| 丁香婷婷成人网| 中文字幕综合| 99re在线播放| 99免费视频网| 影音先锋91男人资源在线播放| www,五月天激情| 伊人激情网| 小视频久久久aaa| 欧美婷婷丁香五月| xxxx久| 久热这里只有精品66| 色九月婷婷丁香| 亚洲视频99| 99热色综合| 深爱激情综合网| 五月丁香综合啪啪啪啪啪| 97人妻超级碰碰碰碰碰| 99精品偷自拍| 五月天狠狠色| 亚洲 综合中文| 久久99精品久久久| 99精品热| 五月婷婷色五月| 色婷婷五月天av在线| 一本色道久久88综合日韩精品| 狠狠干狠狠操狠狠爱| 色五月综合资源推荐| 久久机热这里只有精品免费视频| 成人国产网站| 国产欧美日韩综合精品一区二区| 色五月成人| 四色99久久| 综合久久9| 天天干天天日蜜臀av| 大香蕉网站,大香蕉综合| 开心五月综合| 欧美色99| 婷婷丁香五月激情综合站_久久五月丁香激情综合_开心五月综合激情综合五月_婷 | 婷婷五月婷婷| 777精品成人a v久久| 成人精品人妻| 亚洲Av成人在线观看| 99超级碰免费视频| 丁香六月激情综合| 激情婷婷五月基地| 丁香五月骚喷水视频| 色播播婷婷| 香蕉久久五月| 超碰操日| 亚洲在线操| 成人电影AV在线观看| CHINESE熟女老女人HD视频| 亚洲激情视频在线观看| 色天堂97| 性视频久久| 亚洲中文无码成人| 五月亭久久无码视频| 中文字幕在线资源| 97性视频| 91干视频| 综合99视频| 精品99在线| 丁香五月婷婷激情蜜桃| 99热6精品| 五月婷婷之六月丁香| 中文国产五月天| 国产精品成人av在线观看春天| 日韩成人av在线| 色99在线视频| 丁香五月天色综合| 五月天第四色开心色播| anquye伊人| 亚洲另类婷婷五月综合| 婷婷五月天无码视频| 丁香婷婷激情五月色| 人妻久久久久久久久妻久久久久久久久| 九九久久五月天| 99精品在线观看视频| 色色色婷| 五月天丁香婷婷久久九| 日韩黄色影院| 爱操天堂| 在线观看996精品| 色婷婷成人久久| 中文字幕综合网| 中文字幕成人| 牛牛碰免费| 99无码黄色视频| 激情五月天综合网| 狠狠色噜噜狠狠| 伊人久热91网| 日本综合久| 色色五月丁香| 五月婷婷官网色| 亚洲热热视频| 色色热| 婷婷综合日本| 五月五丁香婷婷| 五月婷婷中文字幕| 色情成人五月天| 成人αV视频免费观看| 在线成人视频免费| www色综合| 色五月婷婷中文字幕| 久久视频在线视频| 色色色区| 欧美天天草人人草| 开心综合激情综合| 亚洲热视频| 91热在线观看视频| 五月天综合影院| 99热综合网| 特黄三级又爽又粗又大| 婷婷久久婷婷色五月| 婷婷色色网| 超碰在线日夜| 六月香五月婷| 婷婷激情四射| 亚洲丁香花色| 综合色影院| 女人天堂AV| 免费AV在线| 天天色天天日| 丁香五月在线观看| 国产麻豆视频| 97色伦另类图片小说视频| 啪到高潮激情丁香五月| 欧美综合激情| 99青青草| 9一精品视频观看| 亚洲 激情 中文| 爽tv | 深情六月婷婷综合久久| 丁香婷五月天开心六月| 激情综合九| 激情婷婷| 亚洲精品视频电影| 一级精品999WWW| www.五月天婷婷| 日韩色色一区| 婷婷九月丁香| 无码少妇高潮喷水A片免费| 五月婷婷天天色| 丰满女老板BD高清A片| 99A级片| 99色亚洲| 狠狠综合网| www.五月丁香| 五月天小说激情| 岛囯综合激情网| 色情综合| 伊人激情AV一区二区三区| av在线激情| 强伦轩人妻一区二区电影| 天天综合网~91| 天天爽天天日天天舔| 久久这里只有精品视频15 | 激情综合网五月| 99日精品视频| 久久99综合| 99在线69| 1024欧美看片| 六月丁香啪啪啪| 伊人网欧美在线男人天堂五月丁香 | 丁香五月婷婷色情综合| 中文无码婷婷| 亚洲黄色网址| 中文字幕婷婷五月天在线观看| 另类综合国产| 久久99热这里只有精品23| 91色九| 九九成人| 亚洲精品久久久久久久久久吃药| 亚洲xx在线| 婷婷日欧美在线观看| 婷婷爱在线观看| 97高清国语自产拍| 婷色五月天| 思思 热 99| 婷婷成人综合| 激情玖玖sh| 婷婷视频在线| 久久色天堂| 5五月综合网亚洲| 91紱請| 欧美久久婷婷| 午夜成人综合| 色播五月婷婷五月| 五月播播| 99精品国产在热久久| 亚洲第一精品成人999久久精品| 九九五月天| av操B网站| 在线观看中文字幕| 亚洲成人av在线播放| 丁香婷婷视频| 婷婷五月天成人网站| 99热超碰| 天天综合情| 五月色婷| 丁香六月婷婷综合| 综合久久高清| 一起肏在线视频| 色吊丝av中文字幕| 久热re视频在线观看网站| 五月天色丁香| 一级黄色影片| 成人AV在线电影| 九九热只有精品| 开心激情站| 日本91在线| 亚洲成人超碰| www.zbzhongsen.com| 99色在线观看视频| 成人亚洲精品| 欧美激情综合| 天天射夜夜爽| 国产67194| 五月天停婷基地| 成人免费视频一区| 只有精品视频在线观看| 久热这里只有精品6官网亚洲| 91色在线/日韩| 99国产97在线,| Www.sesese丁香| 67久久| 中文字幕无码AV| 久久A极片| 日韩啪| 九热视频| 五月天婷婷基地| 色色五月天婷婷| 国产黄色在线| 色色影院黄大片| 日韩无码性爱| 丁香婷婷久久| 五月婷婷开心六月激情小说| 热久久国产视频| 狠狠五月激情丁香六月| 中文AV网| av五月天婷婷丁香| 久久婷婷人人| 丁香五月香蕉在线| 日本激情五月天‘| 五月色婷婷中文字幕| 99色热| 人人操9| 婷婷五月天成人网站| 色天天综合成人网| 中文字幕色色| 色色色9| 91精品激情9| 色老久久| 五月丁香综合久久| 国产在这里只有精品| 啪啪啪大香蕉| OYIWbGcPu8H| 欧美日韩99| 亚洲夜夜操| 99精色| 丁香五月婷婷综合激情哟哟哟| 99热老司机| 伊人午夜综合色啪| 精品香蕉99久久久久网站| www.com亚洲网站在线免费| 婷婷五月激情热播| h在线看免费版在线看| 疯狂做受XXXX高潮A片| 伊人喵咪a V| 丁香婷婷久久| 亚洲狠狠干| 日本三级中国三级99人妇网站| 亚洲另类毛片| 久久五月天激情视频| 色情五月综合婷婷| 97色久| 思思热视频在线观看| 五月丁小婷婷激情四射| 中文字幕日产A片在线看| 久久婷婷东京热大香樵| 五月丁香天堂网| AA片在线观看视频在线播放 | 色婷婷导航| 九九激情网| 色五月天丁香婷婷| 超碰国产AV| 几激情五月婷婷色五月色天堂| 黄网在线免费观| 日韩色色网| 人人摸人人操人人爽| 色色色丁香| 五月丁香婷在线| 国产亚洲成AV人片在线观黄桃| 色婷婷综合视频| 狼人久草| 这里只有精品日韩精品| 国产成人精品一区二三区熟女在线| xxxx五月激情| 欧美大片免费观看| 亚洲欧美999| 极品嫩草| 五月丁香六月激情综合欧美| 大香蕉在线观看9| 六月丁AV| 婷婷五月天激情文学小说| 任你艹| 五月婷婷六月丁香激情深爱| 1024日韩| 亭亭色天香| 月色色综合婷婷网| 国产白丝在线一区| 九九热这里只有精品5| 五月婷婷影院| 开心五月丁香婷婷| 丁香婷婷黄网站| 亚洲天天| 性天天中文网| 另类激情四射| 五月天丁香网| 五月丁香成人网| 武则天精品久久| 蜜桃人妻无码AV天堂三区| 九九热视频免费观看| 五月婷婷丁香综合,亚洲天堂| 97碰碰在线看视频免费| 久久久中文| 色婷婷成人做爰A片免费看网站| 狠狠五月天婷婷激情网。| 五月丁香色综合| 婷婷成人基地| 亚洲丁香五月天视频| 婷婷丁香五另类网站| 久久久99精品免费观看| 色综合色色| 亚洲无码九九| www色色色com| 五月色亚洲| 国产JK精品白丝AV在线观看| 五月丁香啪啪啪综合网| 日韩婷婷五月天| 欧美日韩成人在线观看| 97人碰人操| 亚洲六月婷婷| 成片免费播放| 五月婷婷丁香综合网| 天堂网色色| 色色五月婷婷网| 九九激情网| 五月天激情小说| www.五月天婷婷姐姐| 九九美女视频| 婷婷精品视频| 色爱99| 五月天精品综合| 内射综合网| 亚洲精品大片| 日本成人小说婷婷六月| site:esunnet.com| 久久综合爱| 婷婷丁香色性爱| 久久免费少妇高潮99精品| 久久这里只有精品8| 青青草原伊人网| 激情五月天伊人av| 天天射色五月天| 欧洲婷婷五月天| 亚州欧美国产久精国产99综合视频| 中文久久久人妻| 另类国产综合| 婷婷六月伊人| 五月激情丁香| 久色欧美| 激情5月婷婷| 玖玖热视频| 激情五月天色婷婷| 开心五月婷| 久久曰曰| 国产va在线视频| 日本9区视频| 大香蕉五月天| 激情五月天电影| 99热这里只有在线播放| 亚洲欧洲一二| 午夜天堂一区人妻| 色婷婷激情| 开心四月婷婷在线色播播| 激情婷婷激情在线不卡| 婷婷成人视频| 色婷婷六月| 激情五月激情综合网一级丸片| 97成人在线视频| 伊人五月天| 五月婷婷六月少妇激情| 天天综合色综合| 97香蕉久久超级碰碰高清版| 国产AV网页| 国产精品99久久久久久久女警| 五月天.com| 天天色综合色色色色色。| 综合激情在线| 免费观看日韩成人av| 99热在线播放| 婷婷97碰碰| 丁香五月婷婷成人网| 欧美成人日韩| 久99精品视频| 婷婷内射视频在线| 九九re视频在线视频| 亚洲熟妇AV乱码在线观看 | www.色综合.com| 婷婷五月天激情五月天| 操丝袜视频影院导航| 五月丁香啪啪网| 四色五月视频| 欧美激情-区二区三区| 少妇被躁爽到高潮无码文| 91婷婷| 欧美日韩123| 天天色色婷婷| 五月婷婷五月天天| 人妻久久久久久久久妻久久久久| www.五月婷婷| 亚洲AV综合在线观看| 97操碰在线视频| 99伊人婷婷在线| 日本久久性| 人人综合久| 成人片在线播放| 91一起艹| 女人天堂av| 丁香五月情色| 丁香五月,开心五月,成人婷婷| 在线伦子99热| 日本99热| 色播五月婷婷综合| www,五月天com| 激情综合网五月在线播放| www.色五月.com| 啪啪91| www.色五月| 亚洲九九免费| 五月深爱婷婷| 亭亭社区五月天| 蜜桃五月天色| 久久婷婷综合五月趴| 九九这里有精品| 热久久这里只有精品| 97久久人人| EEUSS鲁片一区二区三区| 色婷婷视频| 日本少妇裸体做爰高潮片| 亚洲精品V天堂中文字幕| av五月天婷婷丁香| 人人操婷婷| 欧美啪啪9| 立川无码av| 婷婷五月天干干| 91碰视频| 极品人妻VIDEOSSS人妻| 丁香五月综合网| 熟女激情网| 婷婷五月天成人动漫 | 五月婷婷六月丁香在线| 337p大胆噜噜噜噜噜91Av| 色综合天天综合成人网| 久久九九激情五月天| 99热传媒| 日韩色色网| 亚洲精品小视频| 婷婷婷久久久| 五月丁香六月婷综合成人综合| 1024久婷| 91精品无码| 思思re视频在线| 五月婷婷之综合激情| 夜夜操夜夜操| 亚洲成人九九九| 99热这里只有精品21| 五月婷亚洲精品| 色玖玖爱| 中文字幕性爱视频| 六月婷婷网| 99视频在线播放大全| 九九综合| 五月天啪啪视频| 99亚州综合精品成人网| 激情婷婷丁香| 久久这里这里有精品免费视频| 亚洲国产精品综合色区| 婷婷激情图片| 五月久久| 99热这里只有精品搜| 婷婷六月丁香五月| 97视频91| 狠狠色大香蕉| 岛国在线观看91| 成人精品一区二区三区四区五区| A1片久久久| 色吧综合网| 妇激情基地| 天天干 夜夜爽| 少妇水多A片太爽了| 天天色·欧美| 97超碰在线免费观看| 色色婷婷婷丁香五月天| 色综合色综合色综合| 国产伦亲子伦亲子视频观看| 色噜噜狠狠色综合网| 日日夜夜小色哥| 婷婷六月色| 五月婷婷六月丁香| 99啪| 超碰猛烈的性猛交| 丁香六月婷婷色播| 五月天激情网址| 激情五月视频在线婷婷| 婷婷大香焦| 九玖欧洲亚洲| 嫩草AV久久伊人妇女超级A| 丁香六月激情综合| 久久五月视频| 久久精品63| 婷婷五月综合激情免费| 日韩精品一曲二曲三曲四曲五曲| 综合色、色综合| 激情五月激情综合网一级丸片| 五月婷婷激情网| 国产成人网站在线观看| 丁香色色网| 六月色婷婷欧美| 免费视频WWW在线观看网站| 久热这里只有精品在线观看 | 秋霞AV淫| 久操综合| 五月天色色网站| 激情小说五月丁香在线视频观看视频| 无码激情AAAAA片-区区| 国产高清RV综合aVa| 成人AV中文字幕| 九月影院義母在线播放| 色偷偷综合| 国产美女最新VA在线免费观看| 九月av在线| 99热99干| 人人草人| 这里只有视频精品| 亚洲中文乱字字幕在线永久| 久久只有精| www.婷婷久久五月天| 九九操操| 五月丁香激情深爱婷婷| 无码人妻电影| 91打屁股免费看| 涩涩五| 青草视频在线播放| 九九成人视频| 五月天成人在线精品| 中文字幕精品推荐免费在线观| 在线婷婷| 亚洲色婷婷视频| 久久久久久久久久91| 国产XXXX搡XXXXX搡麻豆| 六月婷婷五月天| 五月丁香综合激情在线观看| 99热每日| 免费视频WWW在线观看网站| 亚洲成人五月| 婷婷色5月天在线。| 91九色视频在线观看| 97久久人人人干| 日韩啪啪视频| 尔尔AV一区| 99色色最新视频| 99婷婷| 99热精品中文字幕| 五月天激情综合10p| 亚洲区视频| 五月丁香婷婷综合视频| 久久五月天综合| 99热在线这里只有精品| 天天射美女| 丁香五月黄色| 亚洲综合激情五月久久| 五月丁香欧美综合| 国产日产成人亚洲欧美国产VA| 婷婷在线网| 夜夜干夜夜操| 操久久网| 欧州色色| 天天日天天干天天天| 五月婷婷视频ab| 九九家庭影院| 婷婷精品综合| 久久婷婷东京热大香樵| 91色综合久久| 婷婷中文字幕| 久久久99久久| 97久久视频| 99精品视频在线观看| 婷婷五月久久| 婷婷五月天第四色| 99精品视频在线6| 91精品啪| 五月婷婷av在线| 丁香激情综合| 一区二区三区四区无码| 五月久久五月激情| 9l视频自拍9l视频自拍九色学生| 亚洲超级碰| 激情五月婷婷她| 丁香五月手机视频| 成人精品免费在线观看| 狠狠色噜噜狠| 丁香五月 无码| 超碰在线综合| 人人草碰| 影视av久久久噜噜噜噜噜三级| 强伦轩人妻一区二区电影| 色五月琪琪| 成人在线不卡| 99热亚洲精品| 天天综合久久| 高清无码中文字幕aVDV| 婷婷五月激情天| 婷婷婷婷色| 五月开心深爱激情网| 欧美性色视频| 丁香五月婷婷偷拍| 99精品性爱| 日本婷婷| 五月婷婷五月| 草久私拍| 久热69| WWW色综合| 五月的丁香六月的婷婷| 六月丁香五月天| 婷婷五月天激情小说| 日本啪啪天堂| 国产这里只有精品| 99亚洲精品视频| www.婷婷.com| 五月婷婷影| 依人大香蕉在钱1| 五月天婷婷色情| 99免费热在线精品| 9这里只有精品| 99精品视频偷拍| 操人妻AV| 狠狠色狠狠色综合日日91| 色欲婷婷五月天| 婷婷丁香激情五月天色色| 老司机伊人| www.婷婷| 另类视在线| 五月综合视频在线| 五月婷婷之六月丁香| 激情综合五月| 丁香五月婷婷综合啪啪| 色狠狠综合网| 日韩在线婷婷五月天综合| 久久伊人9| 中文字幕人妻一区二区| 久久久999精品| 影音先锋人妻出差| 9|无码久久久久久| 激情文学 综合 九月| 五月婷婷就去色| 色色日本| 久婷婷视平| 99久.| 噜噜噜精品欧美成人在线观看| 99热综合| 婷婷午夜丁香| 五月丁香婷在线| 亚洲成人免费电影| 999热视频精品99免费在线| 免費亭亭成人| 久久综合五月| 丁香五月天激情四射网络不好 | 精品一二三区久久AAA片| 色色亚洲99com| 在线99热| 91成人视频| 九九热精品99| 人人草人人舔| 五月婷狠狠| 免费99情趣网视频| 久婷婷| www.色色com| 五月天婷婷导航| 少妇2做爰HD韩国电影| 都市激情蜜桃婷婷五月天| 久久综合影院| 狠狠狠狠青草| 婷婷天天婷婷天天澡| 婷婷六月色情| 99热资源在线| 久久久ww| 91色婷婷综合久久中文字幕二区| 97影院一级片| 五月婷婷免费在线视频| 色婷婷丁香花五月天| 激情久久四色| 国产亚洲成AV人片在线观黄桃| 国产精品久久在线观看技巧| 日韩在线视频中文字幕| 五月婷精品| 在线另类视频| 五月丁香婷婷成人版| 人人妻久久妻| 99热欧美精品| 亚洲碰碰碰| www.五月天婷婷| av九九| 成人五月天综合网| 激情小说视频图片| 六月亚洲| 伊人大香蕉在线视频| 九九久久99精品免费观看www| 亚州色综合| 免费亚洲婷婷| 久久加勒比| 天天拍天天操| 久久综合天天综合| 婷婷色色宗合网| 亚洲成人五月| 婷婷狠狠操| 亚洲操逼网| 电影爱拉战争免费观看| 美日韩成人| 色婷婷色情| 男女啪啪做爰高潮无遮挡| 亚洲最大视频| 色婷婷色丁香色欲av| 五月天成人免费视频| 激情婷婷人妻| 久久久久久久久久久44| 香蕉久久国产AV一区二区| 五月婷婷丁香综合网| 欧美大香蕉视频| 久久九九思思| 亚洲色五月| 这里精品| 亚洲俩性性爱图片久久第六页| 狠干综合| 人人爽网| 色五月在线播放| 亚洲va在线∨a天堂va欧美va| 国产噜一噜天天噜| 成人毛片在线免费观看| 久久久久久久久久91| 婷婷五月色| 亚洲激情综合| 久久九九re热| 婷婷玖玖五月天| 秋霞少妇毛片| WWW.婷婷| 五月丁香六月激情欧美综合| 天天综合久久| www五月天com| 欧美激情综合色综合啪啪五月| 五月婷婷六月奇米网丁香| 丁香五月综合激情啪啪| 可以免费观看的AV| 中文字幕综合网| 精品亚洲VA网站| www婷婷亚洲| 久久久久9| 日韩五月婷婷| 在线VA视频| 久大香蕉| www.91av.com| 91超级碰碰碰| 五月丁香六月欧美综合网站| 亚洲色就是色色色| 99激情网| 玖玖资源天天无码| 天天看片日日夜夜| 婷婷色色五月| 91爱啪啪| 色天堂97| 91日本在线免费| 亚洲视频一区| 五月天婷婷综合| 91视频精品99| 五月婷婷五月色| 五月婷婷视频在线观看| 性婷婷| 狠狠香蕉| 久久一级免费黄色片| a性生活久久无| 色婷婷很很十八禁| 婷婷五月天国产性感美女演员久久久久| 99视频这里有精品| 欧美性爱一区| 思思久久精品| 狠狠爱综合网| 综合网色| 91久操| 激情五月激情综合网| 五月丁香色婷婷| 婷婷六月综合激情| 啪啪综合网| 天天躁日日躁狠狠躁日日躁2022年5月9日| 五月婷婷成人| 99色综合| 思思久久精品| 被强行糟蹋的女人A片| 欧洲色| 97色吧| 五月丁香婷婷成人网| 国产成人AV不卡| 久久大大香| 激情综合九| 激情综合婷婷久久| 天堂亚洲 在线| 亚洲有码在线视频| 欧美婷婷六月丁香综合色| 亚洲精品一区无码A片| 五月丁香综合激情| 日韩人妻在线播放| 97碰操| 亚洲激情亚洲激情| 就爱射中文字幕资源网| 亚洲综合色棒| www.婷婷五月| 99热草草| 久久色五月| 婷婷五月激情综合| 五月情涩综合婷婷| 婷婷日欧美在线观看| 亚洲综合九九| 欧美色色日韩| www.久久| 夜丁香五月婷婷| 99热免费精品| 五月丁香成人网| 一起草aV| 另类国产综合| 五月天淫乱视频| 亚洲蜜桃精久久久久久久久久久久 | 激情五月天婷婷| 婷婷爱五月天| 性爱网五月婷婷| 久久三级视频| 日日干天天爽| 99超超碰| 综激情网| 色婷婷AV久久久久久久| 激情综合一| 婷五月天丁香婷五月| 天天操天天日天天爱| 婷婷五月中文字幕国产| 丁香六月情| 99色五月| 99这里只有精品在线| 色欲午夜无码久久久久久张津瑜| 99视频综合| peg 2区三区四区的| 99九无网码| 婷婷色五月综合| 狠狠操综合| 日本va视频| 狠狠干无码| 9久9久| 婷婷一本和五月丁香| 激情五月激情综合俺也去婷婷小说| 99久在线精品99re8热| 久久在线视频免费观看| 五月天日日操夜夜操 | 激情久久综合网| 国产精品久久久久久亚洲毛片| 思思99久久| 色中色综合| 丁J香六月首页| 亚洲婷婷乱乱丁香| 五月丁香婷婷欧美色图视频五月丁香777电影| 开心五月婷婷婷美女| 淑女丝袜bi操逼123| 国产黄色av| 99久久er| 五月激情小说| 久久五月综合| 欧美日综合| 97色婷| 五月天色区| 婷婷精品| 亚洲激情综合| 97色婷婷成人综合在线观看| 婷婷丁香五月综合免费视频百花| 国产婷婷五月| 日本美女五月天| 99狠狠| seav天堂| 婷婷五月天成人视频| 五月天婷婷中文字幕在线播放| 午夜福利成人AV91| 五月丁香六月激情| 欧美日韩精品人妻狠狠躁免费视频 | 狠狠丁香| 97色婷婷| 亚洲婷婷激情五月天| 伊人玖玖婷婷| 91精品综合久久久久久五月丁香| 色色啊| 操婷婷基地| www.五月婷婷| 婷婷丁香午夜综合影视| 日本在线观看91| 日韩无码专区| 五月丁花色综合网| 久久久久久久久99精品| 午夜天堂啪啪| 六月丁香婷婷五月| 久久久久久久丁香五月天婷婷| 色婷婷在线综合色播网| 国产做爰视频免费播放| www.久久综合| 天天干天天日日| 97超碰在线免费观看| 五月天激情国产综合婷婷婷| 婷婷色五月色| 成人婷婷桔色| 亚洲一区二区无遮挡A片| 色婷婷六月| 91色逼| 丁香激激情网| 大香蕉五月天| 色天堂操| 三级毛片视频| 深爱五月激情| 国产激情婷婷| 五月综合亚洲| 色婷婷WWW| 丁香五月123| 色婷婷综合久久| 五月丁香偷拍| 99综合| 激情五月综合网最新| 色五月成人婷婷| 狠狠香蕉| 99热色婷婷| 91伦| 国产AV一区二区三区日韩| 久九色| 国产婷婷五月天| 五月天天爽| 天天日天天做天天操| 无码一区二区日韩| 色玖玖爱| 色色丁香婷婷综合| 99热99| 亚洲 欧洲 国产 伦综合| 婷婷五月天性色| 久久er99热精品一区二区| 五月天社区| 婷婷五月六月| 丁香六月欧美| 99久久婷婷| 国产 亚洲 在线| 五月婷婷av在线| 91色婷婷综合久久中文字幕二区| caop在线| 激情五月天视频| 毛片毛片毛片毛片| 九九爱这里只有精品| jiujiu热在线视频| 91日本在线观看| 丁香五月开心亚洲| 超碰猛烈的性猛交| 婷婷综合日本| 久久99久久久| 久久天堂| 天堂中文国产| 九九精品99| 六月婷婷综合| 狠狠爱婷婷| 99ri在线观看视频| 五月丁香免费视频| 超碰91av| av在线免费播放| 日日噜噜夜夜狠狠久久丁香六月| 好叼操在线观看| www.五月丁香| 丁香婷婷影院| 天天爱综合网| 激情五月天综合网| 久久久久人妻| 99热精品中文字幕| 丁香六月婷| 99热综合在线观看| .comwww在线观看免费操| 99九九热视频| 92久久| 天天骑日日爽| 成人欧美Va| 五月婷视屏在线观看| 婷婷丁香色情| 成人丁香五月| 无码 av电影| 奇米四色五月天| 一本九九色| 91精品在线看| 色综合色综合色综合高潮| 色五月丁香六月婷婷| 精品人妻一区二区| 婷婷五月在线观看| 五月婷婷综合激情小说| 97偷拍在线视频| 97精品综合久久| 热这里只有精| 99精品热| 玖玖婷婷五月天| 91超级碰人人操| 97黑人精品区| 天天做天天干天天综合网| 五月天网站免费欧美| 激情六月天| 久9热插入| 五月色婷婷亚洲| 99热这里只有精品18| 日本久久网| 色婷婷亚洲综合天堂| 热中文字幕| 天天爽夜夜爽夜爽精品| 久99久视频| 影音先锋男士资源网一区| 色五月综合在线| 婷婷五月色综合| 亚洲精品视频在线| 九九九九成人| 五月天婷婷在线观看精品男人| 五月婷婷之婷婷| 精品国产一区二区三区四区阿崩| 久婷婷色| 伊人婷婷五月天av| 日日干夜夜干| 99九九视频| 激情内射p| 八戒青柠影视剧在线观看| 91丨九色丨大屁股| 丁香五月影| 狠狠狠狠狠狠狠狠草| 久操热线| 婷婷少妇激情| 五月婷婷丁香俺日污视频| 色婷婷播放| 激情综合亚洲色婷婷五月| 欧美顶级少妇做爰HD| 欧美情月伍月天| 99re久久| 婷婷欧美综合| 99热这里都是精品| 婷婷丁香九色| 五月婷婷五月天天| 亚洲爆乳无码精品AAA片蜜桃| 婷婷丁香色五月亚洲| 亚洲激情四射|