function?[xP]=ekf（fstatexPhmeaszQR）
%?EKF???Extended?Kalman?Filter?for?nonlinear?dynamic?systems
%?[x?P]?=?ekf（fxPhzQR）?returns?state?estimate?x?and?state?covariance?P?
%?for?nonlinear?dynamic?system:
%???????????x_k+1?=?f（x_k）?+?w_k
%???????????z_k???=?h（x_k）?+?v_k
%?where?w?~?N（0Q）?meaning?w?is?gaussian?noise?with?covariance?Q
%???????v?~?N（0R）?meaning?v?is?gaussian?noise?with?covariance?R
%?Inputs:???f:?function?handle?for?f（x）
%???????????x:?“a?priori“?state?estimate
%???????????P:?“a?priori“?estimated?state?covariance
%???????????h:?fanction?handle?for?h（x）
%???????????z:?current?measurement
%???????????Q:?process?noise?covariance?
%???????????R:?measurement?noise?covariance
%?Output:???x:?“a?posteriori“?state?estimate
%???????????P:?“a?posteriori“?state?covariance
%
%?Example:
%{
n=3;??????%number?of?state
q=0.1;????%std?of?process?
r=0.1;????%std?of?measurement
Q=q^2*eye（n）;?%?covariance?of?process
R=r^2;????????%?covariance?of?measurement??
f=@（x）[x（2）;x（3）;0.05*x（1）*（x（2）+x（3））];??%?nonlinear?state?equations
h=@（x）x（1）;???????????????????????????????%?measurement?equation
s=[0;0;1];????????????????????????????????%?initial?state
x=s+q*randn（31）;?%initial?state??????????%?initial?state?with?noise
P?=?eye（n）;???????????????????????????????%?initial?state?covraiance
N=20;?????????????????????????????????????%?total?dynamic?steps
xV?=?zeros（nN）;??????????%estmate????????%?allocate?memory
sV?=?zeros（nN）;??????????%actual
zV?=?zeros（1N）;
for?k=1:N
??z?=?h（s）?+?r*randn;?????????????????????%?measurments
??sV（:k）=?s;?????????????????????????????%?save?actual?state
??zV（k）??=?z;?????????????????????????????%?save?measurment
??[x?P]?=?ekf（fxPhzQR）;????????????%?ekf?
??xV（:k）?=?x;????????????????????????????%?save?estimate
??s?=?f（s）?+?q*randn（31）;????????????????%?update?process?
end
for?k=1:3?????????????????????????????????%?plot?results
??subplot（31k）
??plot（1:N?sV（k:）?‘-‘?1:N?xV（k:）?‘--‘）
end
%}
%?By?Yi?Cao?at?Cranfield?University?02/01/2008
%

[x1A]=jaccsd（fstatex）;????%nonlinear?update?and?linearization?at?current?state
P=A*P*A‘+Q;?????????????????%partial?update
[z1H]=jaccsd（hmeasx1）;????%nonlinear?measurement?and?linearization
P12=P*H‘;???????????????????%cross?covariance
%?K=P12*inv（H*P12+R）;???????%Kalman?filter?gain
%?x=x1+K*（z-z1）;????????????%state?estimate
%?P=P-K*P12‘;???????????????%state?covariance?matrix
R=chol（H*P12+R）;????????????%Cholesky?factorization
U=P12/R;????????????????????%K=U/R‘;?Faster?because?of?back?substitution
x=x1+U*（R‘\（z-z1））;?????????%Back?substitution?to?get?state?update
P=P-U*U‘;???????????????????%Covariance?update?U*U‘=P12/R/R‘*P12‘=K*P12.

function?[zA]=jaccsd（funx）
%?JACCSD?Jacobian?through?complex?step?differentiation
%?[z?J]?=?jaccsd（fx）
%?z?=?f（x）
%?J?=?f‘（x）
%
z=fun（x）;
n=numel（x）;
m=numel（z）;
A=zeros（mn）;
h=n*eps;
for?k=1:n
????x1=x;
????x1（k）=x1（k）+h*i;
????A（:k）=imag（fun

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????文件????????3013??2009-05-20?08:16??ekf.m
?????文件????????1306??2014-02-12?12:33??license.txt