EKF程序，matla

扩展卡尔曼的仿真程序，THIS PROGRAM IS FOR IMPLEMENTATION OF DISCRETE TIME PROCESS EXTENDED KALMAN FILTER FOR GAUSSIAN AND LINEAR STOCHASTIC DIFFERENCE EQUATION.

% THIS PROGRAM IS FOR IMPLEMENTATION OF DISCRETE TIME PROCESS EXTENDED KALMAN FILTER
% FOR GAUSSIAN AND LINEAR STOCHASTIC DIFFERENCE EQUATION.
% By （R.C.R.C.R）SPLABSMPL.
% （17 JULY 2005）.
% Help by Aarthi Nadarajan is acknowledged.
% （drawback of EKF is when nonlinearity is high we can extend the
% approximation taking additional terms in Taylor‘s series）.

clc;  close all; clear all;

Xint_v = [1; 0; 0; 0; 0];
wk = [1 0 0 0 0];
vk = [1 0 0 0 0];

for ii = 1:1:length（Xint_v）
    
    Ap（ii） = Xint_v（ii）*2;
    W（ii） = 0;
    H（ii） = -sin（Xint_v（i