% Kalman filter for the example of free body falling
% Using previous data to modified
clear all
clc
I=eye(360,360);
N=160;
y=[0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 3 4 6 9.8 10 10.2
17.8 26 29 33.4 35.6 53.4 61 70 75.6 81.1 82 82.2 79.8 77.8 79
80.8 79 77.8 71.5 65 62.2 58 55 50.5 48.7 47.7 45.5 42.2 37.8
37.8 34.5 33.4 28.8 25.5 23.3 22.2 21.1 20 17.8 17.1 16.4
15.7 15.5 14.5 13.4 12.2 11.2 10 9.5 9 9.5 10 8.9 5.4
5.5 5.6 5.5 5.4 5.3 5.2 5.1 4.4 4 4 4 4 4 4 3
2 1 0 0.7 1.4 2.2 1.4 0.7 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
dt=60
gg=[19.26 -10.818 0]; hh=eye(1,360);
aa=conv2(hh,gg,'same')
gg=[-6.969 11.818 -3.849]; hh=eye(358,360); bb=conv2(hh,gg,'same')
cc=[0 0 0 0 0 . . . . . 0 -3.12 4.12] % cc大小为(1,360)
C=[aa;bb;cc] ％将aa、bb、cc组合成C矩阵
U=0
M = eye(360)
A = (M/dt)^-1*((M/dt)-C);
U = 0 ;
H = zeros(1,360); H(1,1) = 1;
x=zeros(360,1,N);
x(:,1,1)=[575831
0
0
0
0
.
.
.
.
0]; ％ x(:,1,1)是360*1的矩阵
x0=zeros(1,N)
p=zeros(360,360,N);
p(:,:,1)=eye(360);
F=zeros(360,1);
for k=2:N
x(:,1,k) = A*x(:,:,k-1)+U;
p(:,:,k) = A*p(:,:,k-1)*A';
F(:,1,k) = p(:,:,k)*H' / (H*p(:,:,k)*H');
x(:,1,k)=x(:,:,k)+F(:,1,k)*(y(k-1)-H*x(:,:,k-1));
x0(1,k)=x(230,1,k);
p(:,:,k)=(I-F(:,1,k)*H)*p(:,:,k);;
end
y
x0
t=1:N;
plot(t,y,t,x0);
=====================================
我要求的是x0矩阵的结果，但算出来却是很多NaN...
但是矩阵用手算的话应该是有值的...
昨天查一下 好像有的时候算矩阵会出现这种结果
(可能是分母太接近0的关系??)
也可能是矩阵算出来太大或太小的原因?
不知道要不要换成sym去算=.=..
跪求高手教学...