function?[roll?pitch?yaw?zwx?zwy?zwz]?=?CAL_ATT_EKF（carAccAccXAccYAccZGyrXGyrYGyrZNstNrDeltaT）
????persistent?rollLast?pitchLast?yawLast;
????persistent?GyrOffsetX?GyrOffsetY?GyrOffsetZ;
????%persistent?carSpdLast;
????persistent?P;
%?????persistent?FlagCounter;
%?????persistent?smthAccX?smthAccY?smthAccZ;
????g?=?9.81;
????stDim?=?5;%狀態(tài)維度
????viewDim?=?3;%觀測(cè)維度
????verifyFlag?=?1;%觀測(cè)量是否參與矯正標(biāo)志位
????%初始化
????if?isempty（rollLast）?||?isempty（pitchLast）?||?isempty（GyrOffsetX）?||?isempty（GyrOffsetY）?||?isempty（GyrOffsetZ）
????????rollLast?=?0;?pitchLast?=?0;?yawLast=0;?GyrOffsetX=0;?GyrOffsetY=0;?GyrOffsetZ=0;carSpdLast=0;FlagCounter=0;
????????smthAccX?=?0;?smthAccY?=?0;?smthAccZ?=?0;?
????end
????if?isempty（P）
????????%P?=?diag（[0.01??0.01?0.01?0.01?0.01]）;
????????P?=?diag（[0.01?0.01?0.001?0.001?0.001]）;
????end
????%求出觀測(cè)量矯正標(biāo)志位
%??????FlagCounter?=?FlagCounter+1;
%??????FlagCounter?=?mod（FlagCounter10）;
%??????if?FlagCounter?>?0
%?????????verifyFlag?=?0;
%??????end
%?????if?curspd>1e-6?&&?abs（curspd?-?carSpdLast）>1e-2
%?????????verifyFlag?=?0;
%?????end
%?????if?curspd<1e-6
%??????????GyrOffsetX=0.99*GyrOffsetX?+?0.01*GyrX;?
%??????????GyrOffsetY=0.99*GyrOffsetY?+?0.01*GyrY;?
%??????????GyrOffsetZ=0.99*GyrOffsetZ?+?0.01*GyrZ;???
%?????end
%?????smthAccX?=?0.9?*?smthAccX?+?0.1?*?AccX;
%?????smthAccY?=?0.9?*?smthAccY?+?0.1?*?AccY;
%?????smthAccZ?=?0.9?*?smthAccZ?+?0.1?*?AccZ;
????
????F?=?zeros（stDim6）;
????F（11）?=?1;
????F（12）?=?sin（rollLast）*tan（pitchLast）;
????F（13）?=?cos（rollLast）*tan（pitchLast）;
????F（22）?=?cos（rollLast）;
????F（23）?=?-sin（rollLast）;
????%F（32）?=?-sin（rollLast）*sec（pitchLast）;
????%F（33）?=?cos（rollLast）*sec（pitchLast）;
????W?=?[（GyrX-GyrOffsetX）?（GyrY-GyrOffsetY）?（GyrZ-GyrOffsetZ）?0?0?0]‘;???
????%求雅可比矩陣，對(duì)應(yīng)非線性系統(tǒng)線性化時(shí)F、GW和H
????wx?=?GyrX;wy?=?GyrY;wz?=?GyrZ;zwx?=?GyrOffsetX;zwy?=?GyrOffsetY;zwz?=?GyrOffsetZ;
????fai?=?rollLast;theta=pitchLast;psi=yawLast;
????YF?=?eye（stDim）?+?...
?????DeltaT*[?????cos（fai）*tan（theta）*（wy?-?zwy）?-?sin（fai）*tan（theta）*（wz?-?zwz）???????????????cos（fai）*（tan（theta）^2?+?1）*（wz?-?zwz）?+?sin（fai）*（tan（theta）^2?+?1）*（wy?-?zwy）??-1?-sin（fai）*tan（theta）?-cos（fai）*tan（theta）;
??????????????????????????????-?cos（fai）*（wz?-?zwz）?-?sin（fai）*（wy?-?zwy）?????????????????????????????????????????????????????????????????????????????????????????????0???0????????????-cos（fai）?????????????sin（fai）;
????????????????????????????????????????????????????????????????????????0?????????????????????????????????????????????????????????????????????????????????????????????0???0????????????????????0????????????????????0;
????????????????????????????????????????????????????????????????????????0?????????????????????????????????????????????????????????????????????????????????????????????0???0????????????????????0????????????????????0;
????????????????????????????????????????????????????????????????????????0?????????????????????????????????????????