% 基于_當前_統計模型的目標狀態估計

% 主要靠 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

總結

以上是生活随笔為你收集整理的matlab 状态估计,基于_当前_统计模型的目标状态估计MATLAB 实现的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。