clear?all;
clc;
load（‘data1.mat‘）;

k=size（acc1）;
altitude=zeros（k1）;
velocity=zeros（k1）;

for?t=1:k
????
????baro_alt=（baro（t）-1.6102）*100;????%?turn?to?cm
????acc_z=-（9.80665-acc（t））*100;
????z=[baro_altacc_z]‘;
????[xa_apoPa_apo]=KalmanFilter_core（z0.000000010.000000010.010.000000011000.02dt（t））;
????altitude（t）=xa_apo（1）;
????velocity（t）=xa_apo（2）;
end
x=1:k;
figure;
plot（xaltitude‘r‘‘Linewidth‘2）;
hold?on;
plot（xalt.*100‘g‘‘Linewidth‘2）;
legend（‘KF‘‘PX4‘）;
figure;
plot（x（baro-ones（k1）.*1.6102）.*100‘g‘‘Linewidth‘2）;
hold?on;
plot（xaltitude‘r‘‘Linewidth‘2）;
legend（‘BARO‘‘KF‘）;
figure;
plot（xvelocity）;
%?figure;
%?plot（xacc）;
%?figure;
%?plot（xbaro*100）;

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????文件????????1105??2017-11-10?20:37??KalmanFilter_core.m
?????文件???????13727??2017-11-07?19:46??data.mat
?????文件???????22668??2017-11-07?20:40??data1.mat
?????文件?????????733??2017-11-11?16:53??Altitude_KFm.m