matlab 状态估计,基于_当前_统计模型的目标状态估计MATLAB 实现

% 基于_当前_统计模型的目标状态估计

% 主要靠 kalman滤波实现对物体运动轨迹的估计

%已知运动状态

%初始速度

600 1.匀速10s 2.加速度5，持续10s 3. 匀速5s

%采样周期 1s

% 程序代写&算法设计,联系qq:380238062，转载时请保留

clc

clear

close all

jiasudu=5;%加速度

R1=40; %观测噪声方差

R2=50;

V1=normrnd(0,R1,[25 1]); % A 观测噪声，服从高斯分布，零均值，方差为R1

V2=normrnd(0,R2,[25,1]);

for i=1:25

if

i<=10 %匀速

X(1,i)=600*i; %位置

X(2,i)=600; %速度

X(3,i)=0; %加速度

elseif

i<=20 %匀加速

X(1,i)=6000+600*(i-10)+0.5*jiasudu*(i-10)^2;

X(2,i)=600+jiasudu*(i-10);

X(3,i)=jiasudu;

else

%匀速

X(1,i)=14500+650*(i-20);

X(2,i)=650;

X(3,i)=0;

end

end

[weizhi1,P_P1,X_X1]=kalman_f(X,R1,V1,jiasudu); % kalman滤波

[weizhi2,P_P2,X_X2]=kalman_f(X,R2,V2,jiasudu);

for k=1:25

P1=P_P1{k};

P2=P_P2{k};

X1=X_X1{k};

X2=X_X2{k};

X_zuiyou(:,k)=pinv(P1+P2)*(P2*X1+P1*X2);

%最优加权估计 end

% 程序代写&算法设计,联系qq:380238062，转载时请保留

error1=X(1,:)'-weizhi1';%估计误差

error2=X(1,:)'-weizhi2';

error3=X(1,:)'-X_zuiyou(1,:)';

figure;

plot(error1);

hold on

plot(error2);

plot(error3,':');

text(3,2000,'最优加权误差 - - - - - - - - - - - - - -')

title('估计误差');

function [weizhi,P_P,X_X]=kalman_f(X,R1,V1,jiasudu);

% kalman滤波算法的实现

% 程序代写&算法设计,联系qq:380238062，转载时请保留

a=1/60;

fai=[1 1 (-1+a+exp(a))/(a^2);0 1 (1-exp(-a))/a;0 0

exp(-a)]; % Φ

U=[(-1+a/2+(1-exp(-a))/a)/a 1-(1-exp(-a))/a 1-exp(-a)]';

H=[1 0 0];

I=ones(3,3);

q11=(1-exp(-2*a)+2*a+2*(a^3)/3-2*(a^2)-4*a*exp(-a))/(2*a^5);

q12=(1+exp(-2*a)-2*exp(-a)+2*a*exp(-a)-2*a+a^2)/(2*a^4);

q13=(1-exp(-2*a)-2*a*exp(-a))/(2*a^3);

q22=(4*exp(-a)-3-exp(-2*a)+2*a)/(2*a^3);

q23=(exp(-2*a)+1-2*exp(-a))/(2*a^3);

q33=(1-exp(-2*a))/(2*a);

Q=[q11 q12 q13;

q12 q22

q23;

q13 q23

q33];

X0=[0 600 0]';

P0=1; %初始值

for k=1:25

if

i<=10

a_k=0;

elseif

i<=20

a_k=jiasudu;

else

a_k=0;

end

a_mean=X(1,k)/(k*k); %"当前"平均加速度

if

a_mean==0

sigma_a=1;

else

sigma_a=(4-pi)*(a_mean^2)/2;%“当前”加速度方差

end

Q=2*a*sigma_a*Q;

x_k_k_1=fai*X0+U*a_k;

p_k_k_1=fai*P0*(fai')+Q;

k_k=p_k_k_1*H'/(H*p_k_k_1*H'+R1);

y_k=H*X(:,k)+V1(k);

x_k_k=x_k_k_1+k_k*(y_k-H*x_k_k_1);

p_k_k=(I-k_k*H)*p_k_k_1;

X0=x_k_k;

P0=p_k_k;

P_P{k}=P0;

X_X{k}=x_k_k_1;

weizhi(k)=x_k_k_1(1);

end