組合導航

function [att_open, err, att_res] = fun_filter(X,P,Q,R,q_,gyro_b,acc_b,mag_b,N,acc_n,mag_n)
function [att_open, err, att_res] = fun_move  (X,P,Q,R,q_,gyro_b,acc_b,mag_b,v_b,N,acc_n,mag_n)  %姿態初值q_或cnb_

function [att_open, err, att_res] = fun_speed (X,P,Q,R,q_,gyro_b,acc_b,mag_b,v_b,N,acc_n,mag_n)  %2,3中輸出結果的end = end-1

變量替換 att -> att_open   aa -> att_res

att0 = [0 0 0];
% att0 = [10 10 10];
att0_ = att0-1;
att0 = att0*d_r;
att0_ = att0_*d_r;
% cnb0 = a2cnb(att0);
cnb0_ = a2cnb(att0_);
qe0_ = qmul (qconj(a2qnb(att0)),a2qnb(att0_));

X = [qe0_(2:4)' [10 10 10]/3600*d_r]';
% P = diag([0.01 0.01 0.01 [10 10 10]/3600*d_r].^2);
P = diag([0.01 0.01 0.01 [10 10 10]/3600*d_r].^2);

% Q = diag([10 10 10])/10*0.64;
Q = diag(([0.15 0.15 0.15 10 10 10]/3600*d_r).^2);

R = diag([0.002 0.002 0.002 0.001 0.001 0.001]*0.1);

q_ = m2qnb(cnb0_);