KF_Altitude

function [xa_apo,Pa_apo]=KalmanFilter_core(z,q_baro,q_vel,q_acc,q_bias,r_baro,r_acc,dt) %% init persistent x_apo if(isempty(x_apo)) x_apo=[0,0,0,0]'; end persistent P_apo if(isempty(P_apo)) % P_apo = [0.18756159,0.087983318,0.012799479,-0.0099069523; % 0.087983355,0.072559364,0.027358357,-0.0093248896; % 0.012799491,0.027358379,0.099301167,-0.0042615356; % -0.0099069644,-0.0093249017,-0.0042615356,0.0043873033]; P_apo = single(200*ones(4)); end A=[1 dt 0 0 0 1 dt 0 0 0 1 0 0 0 0 1]; %% copy the states pos=x_apo(1); % position vel=x_apo(2); acc=x_apo(3); bias=x_apo(4); %% prediction section pos=pos+ vel*dt+0.5*acc*dt*dt; vel=vel+ acc*dt; % acc=acc; % bias=bias; x_apr=[pos,vel,acc,bias]'; %states persistent Q if (isempty(Q)) Q=diag([ q_baro,q_vel,q_acc,q_bias]); end P_apr=A*P_apo*A'+Q; %% update R=diag([r_baro,r_acc]); H=[1 0 0 0 0 0 1 1]; s_k=H*P_apr*H'+R; K_k=(P_apr*H'/(s_k)); y_k=z-H*x_apr; x_apo=x_apr+K_k*y_k; P_apo=(eye(4)-K_k*H)*P_apr; %% output xa_apo=x_apo; Pa_apo=P_apo;