平方根容积卡尔曼滤波程序

clear all;
clear all;
clear all;
close all;


clc;
N=100;
beta=1;
ro=0.95;
T=1;
xx（1）=200;
vx（1）=15;
yy（1）=200;
vy（1）=9;
for i=1:N
      if i<=30
        vx（i+1）=15;
        vy（i+1）=9;
    elseif i<40&&i>30
        vx（i+1）=10;
        vy（i+1）=-10;
        ax=-2;
    elseif i<=100&&i>=40
        vx（i+1）=15;
        vy（i+1）=9;
    end
    xx（i+1）=xx（i）+vx（i）*T;%+0.5*0*T^2*randn;%X轴的动态方程
    yy（i+1）=yy（i）+vy（i）*T;
%     z（i+1）=x（i+1）^2+y（i+1）^2;
end
figure;
plot（xxyy‘k‘）;
legend（‘Trajectory‘）;
xlabel（‘X-displacement （m）‘）;
ylabel（‘Y-displacement （m）‘）;
Q=[T^3/3T^2/200;T^2/2T00;00T^3/3T^2/2;00T^2/2T];
R=[600;060];
x（1:）=xx;
x（2:）=vx;
x（3:）=yy;
x（4:）=vy;
n=length（x（:1））;
ckf_kesi1=sqrt（n）*[1000;0100;0010;0001];
ckf_kesi2=-sqrt（n）*[1000;0100;0010;0001];
kesi=[ckf_kesi1ckf_kesi2]

 P=[100000;0100;001000;0001];
    F=[1T00;0100;001T;0001];

w = sqrt（Q）*randn（4N+1）;
v = sqrt（R）*randn（2N+1）;
for