1 Star 0 Fork 2

码头喇叭/NaveGo

forked from mmmmachine/NaveGo 
加入 Gitee
与超过 1200万 开发者一起发现、参与优秀开源项目,私有仓库也完全免费 :)
免费加入
克隆/下载
qua_update.m 2.05 KB
一键复制 编辑 原始数据 按行查看 历史
rodralez 提交于 2019-03-15 10:00 . 2019-03-15
function qua_n = qua_update(qua, wb_n, dt)
% qua_update: updates quaternions.
%
% INPUT:
% qua, 4x1 quaternion.
% wb_n, 3x1 incremental turn rates in body-frame (rad/s).
% dt, 1x1 IMU sampling interval (s).
%
% OUTPUT:
% qua_n, 4x1 updated quaternion.
%
% 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
% <http://www.gnu.org/licenses/>.
% References:
% R. Gonzalez, J. Giribet, and H. Patiño. An approach to
% benchmarking of loosely coupled low-cost navigation systems,
% Mathematical and Computer Modelling of Dynamical Systems, vol. 21,
% issue 3, pp. 272-287, 2015. Eq. 13.
%
% Crassidis, J.L. and Junkins, J.L. (2011). Optimal Esti-
% mation of Dynamic Systems, 2nd Ed. Chapman and Hall/CRC, USA.
% Eq. 7.39 and 7.40, p. 458.
%
% Version: 002
% Date: 2016/11/26
% Author: Rodrigo Gonzalez <rodralez@frm.utn.edu.ar>
% URL: https://github.com/rodralez/navego
wnorm = norm(wb_n);
if wnorm == 0,
qua_n = qua;
else
co=cos(0.5*wnorm*dt);
si=sin(0.5*wnorm*dt);
n1 = wb_n(1)/wnorm;
n2 = wb_n(2)/wnorm;
n3 = wb_n(3)/wnorm;
qw1 = n1*si;
qw2 = n2*si;
qw3 = n3*si;
qw4 = co;
Om=[ qw4 qw3 -qw2 qw1;
-qw3 qw4 qw1 qw2;
qw2 -qw1 qw4 qw3;
-qw1 -qw2 -qw3 qw4];
qua_n = Om * qua;
end
end
马建仓 AI 助手
尝试更多
代码解读
代码找茬
代码优化
Matlab
1
https://gitee.com/mtlb250/NaveGo.git
git@gitee.com:mtlb250/NaveGo.git
mtlb250
NaveGo
NaveGo
master

搜索帮助