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_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_);