func_q.m

来自「移动机器人同时定位与地图创建最前沿技术」· M 代码 · 共 21 行

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% SLAM Summer School 2006, Oxford.
% Practical 3. SLAM using Monocular Vision.
% Practical exercise.
% J.M.M. Montiel, Javier Civera, Andrew J. Davison.
% {josemari, jcivera}@unizar.es, [e-mail]
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [Q,G] = func_Q( Xv,u,Pn,delta_t )

omegaOld=Xv(11:13);
qOld=Xv(4:7);

G=zeros(13,6);

G(8:10,1:3)=eye(3);
G(11:13,4:6)=eye(3);
G(1:3,1:3)=eye(3)*delta_t;
G(4:7,4:6)=dq3_by_dq1(qOld)*dqomegadt_by_domega(omegaOld,delta_t);

Q=G*Pn*G';