EKF卡尔曼算法

扩展卡尔曼滤波算法，具有详细的函数说明，可以根据情况进行调用。

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