复件 (2) kalmandemo.m

kalman滤波器的试验程序

%% kalman filter demo
%% author : LiLei

close all
clear all
clc

% % CovDataObserve = 5;
% % DataN = 1000;
% % DataObserve = zeros( 1 , DataN );
% % DataObserve = 25 + CovDataObserve * randn( 1 , DataN );
% 
% figure(1);
% plot( DataObserve );

N = 1000 ;
w = randn( 1 , N ) ;
x(1) = 25;
for k = 2 : N
    x( k ) = x( k - 1 ) + w( k - 1 );
end

figure(1)
plot( x ,'b');
hold on;

v = 10 * randn( 1 , N );
q1 = std( v );
Rvv = q1.^2;
q2 = std( x );
Rxx = q2.^2;
q3 = std( w );
Rww = q3.^2;

y = 25 + v ;
figure(1);
plot( y , 'r');

p(1) = 10;
s(1) = 10;

for t = 2 : N
    p1( t ) = p( t - 1 ) + Rww;
    b( t ) = p1( t )/( p1( t ) + Rvv );
    s( t ) = s( t - 1 ) + b( t )*( y( t ) - s( t - 1 ) );
    p( t ) = p1( t ) - b( t ) * p1( t );
end

t = 1 : N;
figure(2)
plot( t , s , 'g*' , t , y , 'r' , t , x , 'b' );