Detumbling & Stabalization Bdot algorithm

tags: Internship,NSPO

參考論文

Detumbling & Stablization

FS3_Bdot1：

function [M_bdot,Mdet_sta,ThetaInDegrees] = fcn(B,B_old,K_bdot,K_theta,Alignb, sampleTime,wb, normV)
%% B&Bdot
b = B/norm(B);
b_old = B_old/norm(B);
bdot = (b-b_old)/sampleTime/norm(b);
Bdot = (B-B_old)/sampleTime;

%% 6U Control Therom
if(normV<0.08& bdot~=0)
    %stabilization mode
    temp1 = cross(transpose(wb),B);
    Mdet_sta = transpose(temp1);%ori
else
    if(bdot~=0)
        %detunbling mode
        Mc = -transpose([0,0,0.01]);
        K =  transpose([10000,10000,10000]);
        Mdet_sta = -K.*(bdot)-Mc;
    else
        Mdet_sta = transpose([0 0 0]);
    end
end
CosTheta = dot(Alignb,B)/(norm(Alignb)*norm(B));
ThetaInDegrees = acosd(CosTheta);

Parameter: norm<0.08 go into stablization

模擬結果(thetadegree,3_axis,normwb)

X-Y plan

Detumbling

FS3_Bdot1：

function [M_bdot,Mdet_sta,ThetaInDegrees] = fcn(B,B_old,K_bdot,K_theta,Alignb, sampleTime,wb, normV)
%% B&Bdot
b = B/norm(B);
b_old = B_old/norm(B);
bdot = (b-b_old)/sampleTime/norm(b);
Bdot = (B-B_old)/sampleTime;

%% 6U Control Therom
%detunbling mode
Mc = -transpose([0,0,0.01]);
K =  transpose([10000,10000,10000]);
Mdet = -K.*(bdot)-Mc;

CosTheta = dot(Alignb,B)/(norm(Alignb)*norm(B));
ThetaInDegrees = acosd(CosTheta);

模擬結果(thetadegree,3_axis,normwb)

X-Y plan

Misson :
keep doing