accelerometer.m

来自「MATLAB Example Code : This code shows t」· M 代码 · 共 50 行

clear all
close all
BIAS=.00001*32.2;
SF=.000005;
XK=.000001/32.2;
SIGTH=10E-6;                       %STD DEVIATION
G=32.2;
JJ=0;
count=0;
for THETDEG=0:2:180
    THET=THETDEG/57.3;              %CONVERSION INTO RADIANS
    THETNOISE=SIGTH*randn;          %NORMALLY DIST. NOISE
    THETS=THET+THETNOISE;           %THETA WITH NOISE
    JJ=JJ+1;
    T(JJ)=32.2*cos(THETS);
    X(JJ)=BIAS+SF*G*cos(THETS)+XK*(G*cos(THETS))^2+G*cos(THETS)-G*cos(THET);
end
N=JJ;
SUM1=0; SUM2=0; SUM3=0; SUM4=0; SUM5=0; SUM6=0; SUM7=0; 
for I=1:JJ
    SUM1=SUM1+T(I);
    SUM2=SUM2+T(I)*T(I);
    SUM3=SUM3+X(I);
    SUM4=SUM4+T(I)*X(I);
    SUM5=SUM5+T(I)*T(I)*T(I);
    SUM6=SUM6+T(I)*T(I)*T(I)*T(I);
    SUM7=SUM7+T(I)*T(I)*X(I);
end
A(1,1)=N;  A(1,2)=SUM1;  A(1,3)=SUM2;  
A(2,1)=SUM1;  A(2,2)=SUM2;  A(2,3)=SUM5;  
A(3,1)=SUM2;  A(3,2)=SUM5;  A(3,3)=SUM6; 
AINV=inv(A);
B(1,1)=SUM3; B(2,1)=SUM4; B(3,1)=SUM7;
ANS = AINV*B
for JJ=1:N
            PZ(JJ)=ANS(1,1)+ANS(2,1)*T(JJ)+ANS(3,1)*T(JJ)*T(JJ);
            count=count+1;
            ArrayA(count)=T(JJ);
            ArrayB(count)=X(JJ);
            ArrayPZ(count)=PZ(JJ);
end
figure
plot(ArrayA,ArrayB,ArrayA,ArrayPZ),grid
xlabel('gcos(thet)(deg)')
ylabel('Measurement and Estimate')
axis([-35 35 0 .0005])
clc
output=[ArrayA',ArrayB',ArrayPZ'];
save datfil.txt output -ascii
disp 'simulation finished'