KF_Altitude

个人参照网上资料写得卡尔曼滤波高度融合代码，自测有效，参数任然需要

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