1 / 5

EKF Matlab Equations

This document presents the implementation details of the Extended Kalman Filter (EKF) state update process, focusing on the linearization of the system using MATLAB equations. The state update incorporates a Taylor-Heun approximation, effectively integrating the dynamics and corrections for measurements. The relevant equations outline the state transition model, error covariance propagation, and measurement integration steps. Key concepts include the initialization of state matrices, the prediction phase, and the update mechanism that refines estimates based on new measurements while accounting for uncertainties.

tuan
Télécharger la présentation

EKF Matlab Equations

An Image/Link below is provided (as is) to download presentation Download Policy: Content on the Website is provided to you AS IS for your information and personal use and may not be sold / licensed / shared on other websites without getting consent from its author. Content is provided to you AS IS for your information and personal use only. Download presentation by click this link. While downloading, if for some reason you are not able to download a presentation, the publisher may have deleted the file from their server. During download, if you can't get a presentation, the file might be deleted by the publisher.

E N D

Presentation Transcript

  1. EKF Matlab Equations

  2. , then Let: Problem Setup ,

  3. State Update A(:,:,i)=linearize(qk_km1(:,i)); = F(:,i)=[qk_km1(2,i) (-2*qk_km1(4,i)*qk_km1(3,i)*qk_km1(2,i)-(qk_km1(1,i)*qk_km1(3,i)^2)+ARF(i)) 0 0]';

  4. qk_km1(:,i) = M(:,:,i)*F(:,i)*intdt + qk_km1(:,i); P_dot = A(:,:,i)*Pk_k(:,:,i) + Pk_k(:,:,i)*A(:,:,i)' + Q; Pk_k(:,:,i) = Pk_k(:,:,i) + (intdt)*M(:,:,i)*P_dot*M(:,:,i)'; State Update, cont. Taylor-Heun Approximation: (Mazzoni 2007, courtesy of Muhel & Vivek) M(:,:,i) = inv(eye(4) - A(:,:,i)*intdt/(2));

  5. K(:,i) = Pk_k(:,:,i)*H'*(1/(Pk_k(1,1,i)+R)); qk_k(:,i) = qk_km1(:,i) + K(:,i)*(ytrue(i)-qk_km1(1,i)); Pk_k(:,:,i) = (eye(4) - K(:,i)*H)*Pk_k(:,:,i); Measurement Update

More Related