資源簡介
利用單IMU采集的數據計算當前載體的姿態橫滾角和俯仰角,IMU加計作為觀測量,陀螺作為狀態量。
代碼片段和文件信息
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;%狀態維度
????viewDim?=?3;%觀測維度
????verifyFlag?=?1;%觀測量是否參與矯正標志位
????%初始化
????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
????%求出觀測量矯正標志位
%??????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]‘;???
????%求雅可比矩陣,對應非線性系統線性化時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?????????????????????????????????????????
- 上一篇:基于Labview的微秒延時
- 下一篇:盈通570 4G三星顆粒原版bios
評論
共有 條評論