kalman m文件

 clear
N=1000;
w=1e-5*[randn（1N）;randn（1N）];


m=1395;Iz=6537;a=1.13;b=1.21;k1=-13147;k2=-28318;v=100/3.6;
A=[（k1+k2）/（m*v） （a*k1-b*k2-m*v*v）/（m*v*v）;（a*k1-b*k2）/Iz （a*a*k1+b*b*k2）/（Iz*v）];
B=[-k1/（m*v）;-a*k1/Iz];
C=[（k1+k2）/m （a*k1-b*k2）/（m*v）];
D=[-k1/m];
G=[-a/Iz -b/Iz;
    -1/（m*v） -1/（m*v）];
H=[-1/m -1/m];
V=sqrt（0.01）*randn（1N）;
R=1;
Q=1e-5*[1 0;0 1];
p=zeros（22N）;
x=zeros（2N）;

x（:1）=[0;0];
[A1G1]=c2d（AG0.01）;
[A1B1]=c2d（AB0.01）;
x=zeros（2N）;
x1=[0;0];
for k=2:N;
x1（:k）=A1*x1（:k-1