資源簡介
matlab程序,使用擴展的卡爾曼濾波完成GPS和IMU數據的融合
代碼片段和文件信息
function?[qua_n?DCMbn_n?euler]?=?att_update(wb?DCMbn?qua?omega_ie_N?omega_en_N?dt?att_mode)
%?att_update:?updates?attitude?using?quaternion?or?DCM.
%
%?INPUT:
%???wb?????????3x1?incremental?turn-rates?in?body-frame?(rad/s).
%???DCMbn??????3x3?body-to-nav?DCM.
%???qua????????4x1?quaternion.
%???omega_ie_N?3x1?Earth?rate?(rad/s).
%???omega_en_N?3x1?Transport?rate?(rad/s).
%???dt?????????1x1?INS?time?period?(s).
% att_mode???attitude?mode?string.
%??????‘quaternion‘:?attitude?updated?in?quaternion?format.?Default?value.
%?????????????‘dcm‘:?attitude?updated?in?Direct?Cosine?Matrix?format.
%
%?OUTPUT:
%???qua_n??????4x1?updated?quaternion.
%???DCMbn_n????3x3?updated?body-to-nav?DCM.
%???euler??????3x1?updated?Euler?angles?(rad).
%
%???Copyright?(C)?2014?Rodrigo?Gonzalez?all?rights?reserved.
%
%???This?file?is?part?of?NaveGo?an?open-source?MATLAB?toolbox?for
%???simulation?of?integrated?navigation?systems.
%
%???NaveGo?is?free?software:?you?can?redistribute?it?and/or?modify
%???it?under?the?terms?of?the?GNU?Lesser?General?Public?License?(LGPL)
%???version?3?as?published?by?the?Free?Software?Foundation.
%
%???This?program?is?distributed?in?the?hope?that?it?will?be?useful
%???but?WITHOUT?ANY?WARRANTY;?without?even?the?implied?warranty?of
%???MERCHANTABILITY?or?FITNESS?FOR?A?PARTICULAR?PURPOSE.??See?the
%???GNU?Lesser?General?Public?License?for?more?details.
%
%???You?should?have?received?a?copy?of?the?GNU?Lesser?General?Public
%???License?along?with?this?program.?If?not?see
%??? .
%
%?Reference:
% Crassidis?J.L.?and?Junkins?J.L.?(2011).?Optimal?Esti-
%?mation?of?Dynamic?Systems?2nd?Ed.?Chapman?and?Hall/CRC?USA.
%?Eq.?7.39?p.?458.
%
%?Version:?003
%?Date:????2016/11/26
%?Author:??Rodrigo?Gonzalez?
%?URL:?????https://github.com/rodralez/navego
if?nargin?7?att_mode??=?‘quaternion‘;?end
%%?Correct?gyros?output?for?Earth?rate?and?Transport?rate
wb_n?=?(?wb?-?DCMbn‘?*?(omega_ie_N?+?omega_en_N));
if?strcmp(att_mode?‘quaternion‘)
%%?Quaternion?update???
????qua_n???=?qua_update(qua?wb_n?dt);????%?Update?quaternion
????qua_n???=?qua_n?/?norm(qua_n);??????????%?Brute-force?normalization
????DCMbn_n?=?qua2dcm(qua_n);???????????????%?Update?DCM
????euler???=?qua2euler(qua_n);?????????????%?Update?Euler?angles
????
elseif?strcmp(att_mode?‘dcm‘)
%%?DCM?update????
????
????euler_i?=?wb_n?*?dt;????????????????????%?Incremental?Euler?angles?
????DCMbn_n?=?dcm_update(DCMbn?euler_i);???%?Update?DCM
????euler???=?dcm2euler(DCMbn_n);???????????%?Update?Euler?angles
????qua_n???=?euler2qua(euler);?????????????%?Update?quaternion
????qua_n???=?qua_n?/?norm(qua_n);??????????%?Brute-force?normalization
????
else
????error(‘att_update:?no?attitude?update?mode?defined.‘)
end
end
?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????目錄???????????0??2018-12-12?14:21??NaveGo-master\
?????文件????????5006??2018-12-12?14:21??NaveGo-master\F_update.m
?????文件????????7651??2018-12-12?14:21??NaveGo-master\LICENSE
?????文件???????22414??2018-12-12?14:21??NaveGo-master\README.md
?????目錄???????????0??2018-12-12?14:21??NaveGo-master\allan-variance\
?????文件????????1564??2018-12-12?14:21??NaveGo-master\allan-variance\allan_get_bdrift.m
?????文件????????2158??2018-12-12?14:21??NaveGo-master\allan-variance\allan_get_rw.m
?????文件????????8207??2018-12-12?14:21??NaveGo-master\allan-variance\allan_imu.m
?????文件???????21604??2018-12-12?14:21??NaveGo-master\allan-variance\allan_overlap.m
?????文件????????9963??2018-12-12?14:21??NaveGo-master\allan-variance\dsplot.m
?????文件????????2792??2018-12-12?14:21??NaveGo-master\att_update.m
?????目錄???????????0??2018-12-12?14:21??NaveGo-master\conversions\
?????文件????????1725??2018-12-12?14:21??NaveGo-master\conversions\dcm2euler.m
?????文件????????2486??2018-12-12?14:21??NaveGo-master\conversions\ecef2llh.m
?????文件????????1893??2018-12-12?14:21??NaveGo-master\conversions\ecef2ned.m
?????文件????????1764??2018-12-12?14:21??NaveGo-master\conversions\euler2dcm.m
?????文件????????1861??2018-12-12?14:21??NaveGo-master\conversions\euler2qua.m
?????文件????????1624??2018-12-12?14:21??NaveGo-master\conversions\ned2ecef.m
?????文件????????2110??2018-12-12?14:21??NaveGo-master\conversions\qua2dcm.m
?????文件????????1548??2018-12-12?14:21??NaveGo-master\conversions\qua2euler.m
?????文件????????2037??2018-12-12?14:21??NaveGo-master\conversions\qua2euler_m.m
?????文件????????1351??2018-12-12?14:21??NaveGo-master\conversions\skewm.m
?????文件????????1555??2018-12-12?14:21??NaveGo-master\coriolis.m
?????目錄???????????0??2018-12-12?14:21??NaveGo-master\data-adquition\
?????文件????????9943??2018-12-12?14:21??NaveGo-master\data-adquition\microstrain_read.m
?????文件????????8029??2018-12-12?14:21??NaveGo-master\data-adquition\rinex_read.m
?????文件????????4886??2018-12-12?14:21??NaveGo-master\data-adquition\rtknavi_read.m
?????文件????????4357??2018-12-12?14:21??NaveGo-master\data-adquition\rtkpos_read.m
?????文件????????1622??2018-12-12?14:21??NaveGo-master\dcm_update.m
?????文件????????1538??2018-12-12?14:21??NaveGo-master\earthrate.m
?????目錄???????????0??2018-12-12?14:21??NaveGo-master\examples\
............此處省略44個文件信息
評論
共有 條評論