matlab模拟EKF滤波器

该程序模拟了三维状态非线性系统的EKF算法。

% author :  Perry.Li @USTC
% function: simulating the process of EKF
% date:     04/28/2015
% 
N = 50;         %计算连续N个时刻 
n=3;            %状态维度
Q=0.1^2*eye（n）; %过程方差
R=0.04;         %测量值的方差 
f=@（x）[x（2）;x（3）;0.05*x（1）*（x（2）+x（3））];  %状态方程
h=@（x）[x（1）;x（2）;x（3）];                   %测量方程
s=[0;0;1];                                %初始状态
%初始化状态
x=s+q*randn（31）;                         
P = eye（n）;                               
xV = zeros（nN）;          
sV = zeros（nN）;         
zV = zeros（nN）;
for k=1:N
  z = h（s） + r*randn;                     
  sV（:k）= s;                             %实际状态
  zV（:k）  = z;                           %状态测量值
  [x1A]=jaccsd（fx）;                     %计算f的雅可比矩阵，其中x1对应黄金公式line2
  P=A*P*A‘+Q;                             %过程方差预测，对应line3
  [z1H]=jaccsd（hx1）;                    %计算h的雅可比矩阵
  K=P*H‘*inv（H*P*H‘+R）;                   %卡尔曼增益，对应line4
  x=x1+K*（z-z1）;                          %状态EKF估计值，对应line5
  P=P-K*H*P;                              %EKF方差，对应line6
  xV（:k） = x;                            %save
  s = f（s） + q*randn（31）;                %update process 
end
for k=1:3
  FontSize=14;
  LineWidth=1;
  figure（）;
  plot（sV（k:）‘g-‘）; %画出真实值
  hold on;
  plot（xV（k:）‘b-‘‘LineWidth‘LineWidth） %画出最优估计值
  hold on;
  plot（zV（k:）‘k+‘）; %画出状态测量值
  hold on;
  legend（‘真实状态‘ ‘EKF最优估计估计值‘‘状态测量值‘）;
  xl=xlabel（‘时间（分钟）‘）;
  t=[‘状态 ‘num2str（k）] ;
  yl=ylabel（t）;
  set（xl‘fontsize‘FontSize）;
  set（yl‘fontsize‘FontSize）;
  hold off;
  set（gca‘FontSize‘FontSize）;
end

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件        275  2015-04-26 17:36  jaccsd.m

     文件       1716  2015-04-28 22:08  ekf.m

----------- ---------  ---------- -----  ----

                 1991                    2