¿Qué experto tiene un programa de simulación de Matlab para el algoritmo de filtro de Kalman de navegación combinada GPS y navegación inercial con correa?

En el siguiente código de simulación, la cantidad de observación ideal no son datos reales, sino datos de onda sinusoidal autogenerados. En escenarios de aplicación reales, deben ser una serie de datos de referencia.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Simulación de aplicación de filtro Kalman en navegación integrada INS-GPS

% Autor: lylogn

% Correo electrónico: lylogn@gmail.com

% Empresa: BUAA- Dep3

% Hora: 2013.01.06

%%%%%%%%%% %%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%

% Literatura de referencia:

% '%Configuración del valor inicial de estimación del estado

P=eye(9); %Covarianza de estimación del estado

%% Proceso de iteración del algoritmo de filtro de Kalman

para n=2:longitud

X( :,n)=F*X(:,n-1);

P= F*P*F'+ G*Q*G';

Kg=P* H'/(H*P*H'+R);

X(:, n)=X(:,n)+Kg*(Z(:,n)-H*X(: ,n));

P=(eye(9,9)-Kg*H )*P;

fin

%% código de dibujo

figura(1)

trama(y(1,:))

espera;

trama(y(2,:) )

esperar;

title('Cantidad de observación ideal ');

figura(2)

plot(Z(1 ,:))

espera;

plot(Z (2,:))

espera;

title(' Observaciones con ruido');

figura(3)

plot(X(1,:))

espera;

plot(X(2,:))

esperar;

plot(X(2,:))

esperar;

title('Observaciones filtradas');