function [xmais,Pmais Kmais] = kalman_estendido(xest,yplanta,Pk,Q,R,dt,A,C)
%Código: Filtro de Kalman estendido
%Autor: Guilherme Gonçalves
%data: 19/04/2013
%-----------------------------------------------
%Dados de entrada
%----------------------------------------------
%xest = predição feita com o modelo não linear
%yplanta = medição da planta
%Pk = matriz de covariância a ser propagada 
%Q = matriz de pesos
%R = matriz de pesos
%dt = intervalo de discretização
%A = matriz jacobiana no instante de tempo atual
%C = equação de medição 
%-----------------------------------------------
%Dados de saida
%----------------------------------------------
%xmais = estado corrigido
%Pmais = Matriz de covariância atualizada
%Kmais = ganho de Kalman

%----------------------------------------------



%carregamento das matrizes do sistema e calculo da matriz exponencial
%(Discretização do sistema)

MAe = A;
MCe = C;

phi = expm(MAe*dt); 

%calculo da matriz de covariância
%Pmenos = phi*Pk*phi' - ((phi*Pk)*MCe')*inv((MCe*Pk)*MCe' + R)*(MCe*(Pk*phi')) + Q; %calculo discreto
Pmenos = phi*Pk*phi' - ((phi*Pk)*MCe')/((MCe*Pk)*MCe' + R)*(MCe*(Pk*phi')) + Q; %calculo discreto

%cálculo do ganho de Kalman
Kmais = (Pmenos*MCe')/(MCe*Pmenos*MCe' + R);

%atualização do estado
xmais = xest' + Kmais*(yplanta' - C*xest');

%atualização da matriz de covariância
Pmais = Pmenos - Kmais*MCe*Pmenos;
