卡尔曼滤波算法的matlab 实现

卡尔曼滤波算法 的MATLAB实现 压缩包直接打开即可

clear all;close all;clc;
ts=1;%%采样时间
t=[0:ts:100];
T=length（t）;
Q=20;R=16;
vs=randn（1T）;
vs=vs/std（vs）;
vs=vs-mean（vs）;
a=0;
b=sqrt（R）;
vs=a+b*vs;%产方差为16的高斯白噪声序列;
wa=randn（1T）;
wa=wa/std（wa）;
wa=wa-mean（wa）;
a=0;
b=sqrt（Q）;
wa=a+b*wa;%产方差为20的高斯白噪声序列;
%%初始状态
x=[1000 50 20]‘;%
x00=[990;0;0];%
p00=[10 0 0;0 100 0;0 0 100];
%%连续状态空间模型
%x_dot（t）=Ax（t）+Bu（t）
%z（t）=Cx（t）+Du（t）;
A=[0 1 0;0 0 1;0 0 0];%
B=[0;0;1];%
C=[1 0 0];%
D=[1];%
%%离散状态空间模型
%x（k+1）=Fx（k）+Gw（k）
%z（k）=Hx（k）+Iv（k）;
%连续形式的离散化
sysc=ss（ABCD）;
sysd=c2d（syscts‘zoh‘）;
[F G H I ]=ssdata（sysd）;
% F=eye（3）+A;
phi=F;
for i=1:T-1
    x（:i+1）=F*x（:i）;
end
%x=x+w;
m=0;
x=x+G*wa;
z=H*x+I*vs;%计算观测值

for d=1:T-1
x10=phi*x00;
p10=phi*p00*phi‘+G*20*G‘;
K=p10*H‘*inv（H*p10*H‘+R）;
x11=x10+K*（z（:d）-H*x10）;
p11=p10-K*H*p10;
    p00=p11;
    x00=x11;
    jie（d）=x11（1）;%位置
    su（d）=x11（2）;%速度
    jia（d）=x11（3）;%加速度
   
end     %卡尔曼滤波
for d=1:T-1
  weicha（d）=jie（d）-x（1d）; %位置误差
   sucha（d）=su（d）-x（2d）;%速度误差
   jiacha（d）=jia（d）-x（3d）;%加速度误差
end
t=0:1:T-2;
d=0:1:T-1;
figure（1）;
plot（tjie‘g+‘）;
hold on;
plot（dx（1:）‘r‘）;
figure（2）;
plot（tsu‘g+‘）;
hold on;
plot（dx（2:）‘r‘）;
figure（3）;
plot（tjia‘g--‘）;
hold on;
plot（dx（3:）‘r‘）;
figure（4）
plot（tweicha）;
figure（5）
plot（tsucha）;
figure（6）;
plot（tjiacha）;
mean1=mean（jia）%加速度均值
mean2=mean（weicha）%位置误差均值
mean3=mean（sucha）%速度误差均值
mean4=mean（jiacha）%加速度误差均值
jiuncha1=std（jia）^2%加速度方差
jiuncha2=std（weicha）^2%位置误差方差
jiuncha3=std（sucha）^2 %速度误差方差
jiuncha4=std（jiacha）^2%加速度误差方差

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

     文件     133632  2015-06-20 23:02  辛弄潮 201230050602021201.doc

     文件       1725  2015-06-20 22:58  test.m

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

               135357                    2