getr.m

来自「文件包含有5项内容： 一、扩展卡尔曼滤波EKF 二、去偏转换卡尔曼滤波CMK」· M 代码 · 共 44 行

function [R]=GetR(rm,sitam,betam,DeltaR,DeltaSita,DeltaBeta);

DeltaR2=DeltaR^2;
DeltaSita2=DeltaSita^2;
DeltaBeta2=DeltaBeta^2;

eSita1=exp(-DeltaSita2);
eSita2=eSita1^2;
eSita3=eSita1^3;
eSita4=eSita1^4;

eBeta1=exp(-DeltaBeta2);
eBeta2=eBeta1^2;
eBeta3=eBeta1^3;
eBeta4=eBeta1^4;

    %临时变量c2B,s2S,cB sB cS,sS
    c2B=cos(2*betam);
    s2S=sin(2*sitam);
    s2B=sin(2*betam);
    c2S=cos(2*sitam);
    
    cB=cos(betam);
    sB=sin(betam);
    cS=cos(sitam);
    sS=sin(sitam);
    
    %临时变量r2 r1
    r2=rm^2+2*DeltaR2;
    r1=rm^2+DeltaR2;
    
    R11= (r2*(c2B*c2S*eSita4*eBeta4+c2B*eBeta4+c2S*eSita4+1)-r1*(c2B*c2S*eSita3*eBeta3+c2B*eSita1*eBeta3+c2S*eSita3*eBeta1+eBeta1*eSita1))/4;
    R22=(-r2*(c2B*c2S*eSita4*eBeta4-c2B*eBeta4+c2S*eSita4-1)+r1*(c2B*c2S*eSita3*eBeta3-c2B*eSita1*eBeta3+c2S*eSita3*eBeta1-eSita1*eBeta1))/4;
    R33=(r1*(c2B*eBeta3-eBeta1)-r2*(c2B*eBeta4-1))/2;
    R12=s2S*eSita2*(r2*eSita2*(1+eBeta4*c2B)-r1*eSita1*eBeta1*(1+eBeta2*c2B))/4;
    R13=cos(sitam)*s2B*(-r1+r2*eBeta1)*eSita1*eBeta3/2;
    R23=sin(sitam)*s2B*(-r1+r2*eBeta1)*eSita1*eBeta3/2;


      R=[R11   R12  R13;
         R12   R22  R23;
         R13   R23  R33];