yunsu_kalman_two_dim.m

目标匀速运动的卡曼滤波的matlab代码

%设目标沿X轴作匀速直线运动
%状态方程
%X（k）=AX（k-1）+W（k）；
%Y（K）=HX（K）+V（k）；
%E(v(k))=Q;
%E(w(k))=R
clear all
clc;
close all
T=1;
A=[1 T 0 0;0 1 0 0;0 0 1 T;0 0 0 1];
H=[1 0 0 0;0 0 1 0];
N=500;
%x=zeros(4,N);
x=zeros(4,N);
w=randn(N,2);
v=randn(N,4);
%v=zeros(1,N);
R=cov(w);
Q=cov(v);
object_state=[1:N;1:N];
z=object_state+w';
x(:,1)=[0 1 0 1]';
p=ones(4,4);
G=eye(4);
for k=2:N
    x(:,k)=A*x(:,k-1);
    p=A*p*A'+Q;
    K=p*H'*inv(H*p*H'+R);
    x(:,k)=x(:,k)+K*(z(:,k)-H*x(:,k));
    p=(eye(4)-K*H)*p;
end
figure
%plot(object_state);
%hold on;
plot(x(1,:));
figure
plot(x(2,:))
figure
plot(x(3,:))
figure
plot(x(4,:))
figure
plot(z(1,:),z(2,:))
figure
plot(object_state(1,:),object_state(2,:))
figure
plot(x(1,:),x(3,:))