加速度计原始数据卡尔曼滤波

clear
N=3000;
w（1）=0;
q = 1e-3;
w=CreateGauss（0q1N）;
x（1）=0;
a=1;
for k=2:N;
x（k）=a*x（k-1）+w（k-1）;
end

r = 5.63;
%V=CreateGauss（0r1N）;
%q1=std（V）;
q1 = 2.3;
q2=std（x）;
Rvv=q1.^2;
Rxx=q2.^2;
q3=std（w）;
Rww=q3.^2;
c=1;

fid = fopen（‘x.txt‘‘r‘）;
for i = 1:1
    y = fscanf（fid‘%f‘）;
 %   y = y-33.6316;
    y = y‘;
end


p（1）=1e-1;
o（1）=1;
s（1） = 0;
v（1） = 0;
for t=2:N;
p1（t）=a.^2*p（t-1）+Rww