这是一个关于非线性扩展卡尔曼滤波器（EKF）的代码教程。
This is a tutorial on nonlinear extended Kalman filter (EKF).
它使用标准的EKF仿真来实现非线性状态估计。
It uses the standard EKF fomulation to achieve nonlinear state estimation.
利用复步长雅可比矩阵对非线性动态系统进行线性化。
Inside, it uses the complex step Jacobian to linearize the nonlinear dynamic system.
从而可以在卡尔曼滤波器的计算中使用线性矩阵。
The linearized matrices are then used in the Kalman filter calculation.
完整源码如下：
function [x,P]=ekf(fstate,x,P,hmeas,z,Q,R)
% EKF Extended Kalman Filter for nonlinear dynamic systems
% [x, P] = ekf(f,x,P,h,z,Q,R) 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(0,Q) meaning w is gaussian noise with covariance Q
% v ~ N(0,R) 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.05x(1)(x(2)+x(3))]; % nonlinear state equations
h=@(x)x(1); % measurement equation
s=[0;0;1]; % initial state
x=s+q*randn(3,1); %initial state % initial state with noise
P = eye(n); % initial state covraiance
N=20; % total dynamic steps
xV = zeros(n,N); %estmate % allocate memory
sV = zeros(n,N); %actual
zV = zeros(1,N);
for k=1:N
z = h(s) + r*randn; % measurments
sV(:,k)= s; % save actual state
zV(k) = z; % save measurment
[x, P] = ekf(f,x,P,h,z,Q,R); % ekf
xV(:,k) = x; % save estimate
s = f(s) + q*randn(3,1); % update process
end
for k=1:3 % plot results
subplot(3,1,k)
plot(1:N, sV(k,:), ‘-’, 1:N, xV(k,:), ‘–’)
end
%}
% By Yi Cao at Cranfield University, 02/01/2008
%
[x1,A]=jaccsd(fstate,x); %nonlinear update and linearization at current state
P=APA’+Q; %partial update
[z1,H]=jaccsd(hmeas,x1); %nonlinear measurement and linearization
P12=P*H’; %cross covariance
% K=P12inv(HP12+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-UU’; %Covariance update, UU’=P12/R/R’P12’=KP12.
function [z,A]=jaccsd(fun,x)
% JACCSD Jacobian through complex step differentiation
% [z J] = jaccsd(f,x)
% z = f(x)
% J = f’(x)
%
z=fun(x);
n=numel(x);
m=numel(z);
A=zeros(m,n);
h=n*eps;
for k=1:n
x1=x;
x1(k)=x1(k)+h*i;
A(:,k)=imag(fun(x1))/h;
end
完整源码下载地址：
http://page5.dfpan.com/fs/8lc3j2221a29916af91/
更多精彩文章请关注微信号：