% 基于_当前_统计模型的目标状态估计
% 主要靠 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