reference.m

通用的unscented 卡尔曼滤波器是现在应用最广泛的非线性卡尔曼滤波器 又叫做sp 卡尔曼滤波器

function reference()

close all;
clear all;

x0 = [300000; -20000; 0.001];
xture = x0;
dt = 0.01;   
Rho = 2;          
g = 32.2;   
k = 20000;       
M = 100000;
a = 100000;

NN = 3000;

for i=1:NN
   
    xture = feval(@predict_model,xture,Rho,g,k,dt);
    yture = feval(@observe_model,xture,M,a);

       
    Xture(:,i)=xture;
    Yture(i)=yture;
end

    figure();
    plot(Xture(3,:),'r-');
    hold on;
    figure();
    plot(Xture(1,:),'k-');
    hold on;
    figure();
    plot(Xture(2,:),'b-');
    hold on;
    figure();
    plot(Yture,'r-');
    hold on;

save reference Xture Yture;




function x = predict_model(x,Rho,g,k,dt)
x = [x(1,:)+x(2,:)*dt;
    x(2,:)+dt*Rho*exp(-x(1,:)/k).*(x(2,:).^2).*x(3,:)/2-dt*g;
    x(3,:) ];


function y = observe_model(x,M,a)
temp1=size(x);
n=temp1(2);
M=repvec(M,n);
a=repvec(a,n);
y = sqrt(M.^2+(x(1,:)-a).^2);

function x = repvec(x,N)
x = x(:, ones(1,N));