imm_extend2()
%交互式多模型
初步改进
clear
all
close
all
timenum=75
t=1
transfer_matric=[0.98
0.02
%马尔科夫转移矩阵
矩阵为j行i列
0.02
0.98]
u_10=0.5
%目标1在模型i在k-1时刻的概率
u_20=0.5
%目标1在模型i在k-1时刻的概率
u=[u_10
u_20]
Q1=100
Q2=4000
%目标1的状态噪声协方差阵
R=10000
%观测噪声协方差阵
%R2=700
x00_1=[0
0
0]'
%模型1估计的初始值
x00_2=[0
0
0]'
%模型2估计的初始值
p00_1=[100
0
0
0
10
0
0
0
0]
p00_2=[100
0
0
0
10
0
0
0
10]
dis=1000
%位移
vel=450
%速度
acc=50
%加速度
xx01=[dis
vel
0]'
%状态的初始值
xx02=[dis
vel
acc]'
%状态的初始值
%xx2=0
f1=[1
t
0
0
1
0
0
0
0]
%状态转移阵
f2=[1
t
0.5*t^2
0
1
t
0
0
1]
l1=[0.5*t^2
t
0]'
%状态干扰阵
l2=[0.5*t^2
t
1]'
h=[1
0
0]
%观测转移阵
I=[1
0
0
0
1
0
0
0
1]
num=1
XX1=zeros(3,25)
XX2=XX1
XX3=XX1
while
num<=timenum
if
num<=25
xx1=f1*xx01+l1*sqrt(Q1)
%目标的状态值
XX1(:,num)=xx1
z(num)=h*xx1+sqrt(R)*randn(1)
%目标的观测值
else
if
25<num<=50
xx2=f2*xx02+l2*sqrt(Q2)
%目标的状态值
XX2(:,num-25)=xx2
z(num)=h*xx2+sqrt(R)*randn(1)
%目标的观测值
else
if
50<num<=timenum
xx3=f1*xx01+l1*sqrt(Q1)
%目标的状态值
XX3(:,num-50)=xx3
z(num)=h*xx3+sqrt(R)*randn(1)
%目标的观测值
end
end
end
%================input
alternation==============================
tran_11=transfer_matric(1,1)*u(1)
tran_12=transfer_matric(1,2)*u(1)
tran_21=transfer_matric(2,1)*u(2)
tran_22=transfer_matric(2,2)*u(2)
sum_tran_j1=tran_11+tran_21
sum_tran_j2=tran_12+tran_22
sum_tran_j=[sum_tran_j1
sum_tran_j2]
u_mix(1,1)=tran_11/sum_tran_j1
u_mix(1,2)=tran_12/sum_tran_j2
u_mix(2,1)=tran_21/sum_tran_j1
u_mix(2,2)=tran_22/sum_tran_j2
%
for
i=1:2
%
x00_estimate_j(i)=sum_tran_j(i)*x00(i)
%
p00_estimate_j(i)=sum_tran_j(i)*(p00+(x00-x00_estimate_j(i))*(x00-x00_estimate_j(i))')%p00_estimate以行存储
%end
x00_estimate_j(:,1)=x00_1*u_mix(1,1)+x00_2*u_mix(2,1)
x00_estimate_j(:,2)=x00_1*u_mix(1,2)+x00_2*u_mix(2,2)
p00_estimate_j1=(p00_1+(x00_1-x00_estimate_j(:,1))*(x00_1-x00_estimate_j(:,1))')*u_mix(1,1)+...
(p00_2+(x00_2-x00_estimate_j(:,1))*(x00_2-x00_estimate_j(:,1))')*u_mix(2,1)
p00_estimate_j2=(p00_1+(x00_1-x00_estimate_j(:,2))*(x00_1-x00_estimate_j(:,2))')*u_mix(1,2)+...
(p00_2+(x00_2-x00_estimate_j(:,2))*(x00_2-x00_estimate_j(:,2))')*u_mix(2,2)
%==================filter
calculate=================================
%================model
1===================================
x1_pre=f1*x00_estimate_j(:,1)
p1_pre=f1*p00_estimate_j1*f1'+l1*Q1*l1'
s1=h*p1_pre*h'+R
k_gain1=p1_pre*h'*inv(s1)
p1_estimate=(I-k_gain1*h)*p1_pre
v1=z(num)-h*x1_pre
x1_estimate=x1_pre+k_gain1*v1
X1_estimate(:,num)=x1_estimate
%================model
2===================================
x2_pre=f2*x00_estimate_j(:,2)
p2_pre=f2*p00_estimate_j2*f2'+l2*Q2*l2'
s2=h*p2_pre*h'+R
k_gain2=p2_pre*h'*inv(s2)
p2_estimate=(I-k_gain2*h)*p2_pre
v2=z(num)-h*x2_pre
x2_estimate=x2_pre+k_gain2*v2
X2_estimate(:,num)=x2_estimate
%===============================================================
%===================model
probability
alternate=================
p_pre=[p1_prep2_pre]
x_pre=[x1_pre'
x2_pre']
s=[s1(1)
s2(1)]
v=[v1
v2]
for
i=1:2
model_fun(i)=(1/sqrt(2*pi*s(i)))*exp(-v(i)*v(i)'/(2*abs(s(i))))
end
sum_tran=model_fun(1)*sum_tran_j1+model_fun(2)*sum_tran_j2
u_j1=model_fun(1)*sum_tran_j1/sum_tran
u_j2=model_fun(2)*sum_tran_j2/sum_tran
u_j=[u_j1
u_j2]
%==============================================================
%====================output
alternation=========================
x_sum=x1_estimate*u_j(1)+x2_estimate*u_j(2)
X_sum(:,num)=x_sum
p_sum=(p1_estimate+(x_sum-x1_estimate)*(x_sum-x1_estimate)')*u_j1+(p2_estimate+(x_sum-x2_estimate)*(x_sum-x2_estimate)')*u_j2
P_sum(3*num-2:3*num,1:3)=p_sum
%x00=x_sum(k)
%x00=[x1_estimate(k)
x2_estimate(k)]
x00_1=x1_estimate
x00_2=x2_estimate
%p00=[p1_estimate(k)
p2_estimate(k)]
p00_1=p1_estimate
p00_2=p2_estimate
if
(num<=25|25<num<=75)
xx01=xx1
else
if
25<num<=50
xx02=xx2
%else
%
xx01=xx3
end
end
%xx2=xx_2(k)
u=[u_j1
u_j2]
num=num+1
end
XX=[XX1
XX2
XX3]
figure
plot(XX(1,:)','b')
hold
on
plot(X_sum(1,:),'m')
legend('状态','融合输出的估计')
plot(z,'r')
xlabel('采样次数')
ylabel('相应值')
grid
hold
off
%figure
%plot(p1_estimate,'b')
%hold
on
%plot(p2_estimate,'m')
%plot(p_sum,'r')
%hold
off
%legend('模型1的协方差','模型2的协方差','融合输出的协方差')
%xlabel('采样次数')
%ylabel('相应值')
%grid
figure
plot(XX(2,:)','b')
hold
on
plot(X_sum(2,:)','r')
legend('状态','融合输出的估计')
xlabel('采样次数')
ylabel('相应值')
grid
hold
off
figure
plot(XX(3,:),'b')
hold
on
plot(X_sum(3,:)','r')
legend('状态','融合输出的估计')
xlabel('采样次数')
ylabel('相应值')
grid
hold
off
X_sum
XX
im=imread('7.jpg')im=rgb2gray(im)
imm=double(im)
figure,imagesc(imm),colormap(gray)
hist(imm)figure(gcf)
im1=imm<=180
figure,imagesc(im1),colormap(gray)
im2=im1.*imm
figure,imagesc(im2),colormap(gray)
欢迎分享，转载请注明来源：内存溢出
微信扫一扫
支付宝扫一扫
评论列表（0条）