main_general_ukf.asv

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

function main_general_ukf()

clc;
clear all;
close all;

load reference;

% Given data
x0 = [300000; -20000; 0.001];
P = [1000000 0 0;0 4000000 0;0 0 10];
xe = x0;
w=[100;10;0.0001];   
%w=[10;1;0.00001];
%w=[1;0.1;0.00001];
%w=[0.1;0.01;0.000001];
%w=[0.01;0.001;0.0000001];
%w=[0.001;0.0001;0.00000001];
Q = [w(1)^2 0 0;0 w(2)^2 0;0 0 w(3)^2];  
dt = 0.01;       
R =100000;
u0 = [0;0;0];

% Filter loop
N = 3000;
for i=1:N
    y = multivariate_gauss_noise(Yture(i), R, 1);
    u = multivariate_gauss_noise(u0, Q, 1);
    
    % predict (time update equations)
    [xe,P] = predict(xe,P,u,Q,dt);
   
    % update (measurement update equations)
    [xe,ye,P] = update(xe,y,P,R);
       
    X(:,i)=xe;
    Y(i)=ye;
end
    
    general_ukf_errorX=X-Xture;
    general_ukf_errorY=Y-Yture;
    general_ukf_errorX(:)
    
    figure();
    
    subplot(3,1,1);
    plot(X(1,:),'k-');
    xlabel('time(10ms)'),ylabel('altitude(ft)');title('general ukf estimated altitude');
    hold on;
    
    subplot(3,1,2);
    plot(X(2,:),'b-');
    xlabel('time(10ms)'),ylabel('velocity(ft/s)');title('general ukf estimated velocity');
    hold on;
    
    subplot(3,1,3);
    plot(X(3,:),'r-');
    xlabel('time(10ms)'),ylabel('ball.coeff.');title('general ukf estimated ball.coeff.');
    hold on;
    
    
    figure();
    subplot(3,1,1);
    plot(general_ukf_errorX(1,:),'k-');
    xlabel('time(10ms)'),ylabel('altitude error(ft)');title('general ukf estimated altitude error');
    hold on;
    
    subplot(3,1,2);
    plot(general_ukf_errorX(2,:),'b-');
    xlabel('time(10ms)'),ylabel('velocity error(ft/s)');title('general ukf estimated velocity error');
    hold on;
    
    subplot(3,1,3);
    plot(general_ukf_errorX(3,:),'r-');
    xlabel('time(10ms)'),ylabel('ball.coeff. error');title('general ukf estimated ball.coeff. error');
    hold on;

save general_ukf_error general_ukf_errorX general_ukf_errorY;



function [xe,P] = predict(xe,P,u,Q,dt)
xe = [xe; u];
P = blkdiag(P,Q);
[xe,P] = unscented_transform(@predict_model,[],xe,P,dt); 

function [xe,ye,P] = update(xe,y,P,R)
[xe,ye,P] = unscented_update(@observe_model,@observe_residual,xe,y,P,R);

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


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

function v = observe_residual(z1, z2)
% Given two observation values, compute their normalised residual.
% Notice, once again, that the model is vectorised in terms of z1 and z2.
v = z1-z2;

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