%%clear the way
% author : liuweimin
% ictcas
close all
clear
clc
%% settings
M=3 % number of Gaussian
N=1000% total number of data samples
th=1e-3 % convergent threshold
Nit=2000 % maximal iteration
Nrep=10% number of repetation to find global maximal
K=2 % demention of output signal
pi=3.141592653589793% in case it is overwriten by smae name variable
cond_num =1000 % prevent the singular covariance matrix in simulation data
plot_flag=1
print_flag=1
%% paramethers for random signal genrator
% random parameters for M Gaussian signals
mu_real = randn(K,M)% mean
cov_real =zeros(K,K,M) % covariance matrix
covd_real=zeros(K,K,M) % covariance matrix decomposition
for cm=1:M
while 1
covd_real(:,:,cm)=randn(K,K)
cov_real(:,:,cm)=covd_real(:,:,cm)*covd_real(:,:,cm)'
if cond(cov_real(:,:,cm))>cond_num
continue
else
break
end
end
end
% probablilty of a channel being selected
a_real = abs(randn(M,1))
a_real = a_real/sum(a_real) % normlize
if print_flag==1
a_real%类别概率的真值
mu_real%均值的真值
cov_real%协方差的真值
end
%% generate random sample of Gaussian vectors
%m=randdist(1,N,[1:M],a_real) % selector
rand_num_a=rand()%产生随机数
rand_num_b=rand()%产生随机数
while rand_num_a>=rand_num_b
rand_num_a=rand()%产生随机数
rand_num_b=rand()%产生随机数
end
x=randn(K,N)
for c=1:round(rand_num_a*N)
%sel=m(c)
x(:,c)=covd_real(:,:,1)*x(:,c)+mu_real(1)
end
for c=round(rand_num_a*N)+1:round(rand_num_b*N)
%sel=m(c)
x(:,c)=covd_real(:,:,2)*x(:,c)+mu_real(2)
end
for c=round(rand_num_b*N)+1:(N)
%sel=m(c)
x(:,c)=covd_real(:,:,3)*x(:,c)+mu_real(3)
end
%% EM Algorothm
% loop
f_best=-inf
for crep=1:Nrep
c=1
% initial values of parameters for EM
a=abs(randn(M,1)) % randomly generated
a=a/sum(a)% normlize, such that sum(a_EM)=1
mu=randn(K,M)
cov =zeros(K,K,M) % covariance matrix
covd=zeros(K,K,M) % covariance matrix decomposition
for cm=1:M
while 1
covd(:,:,cm)=randn(K,K)
cov(:,:,cm)=covd(:,:,cm)*covd(:,:,cm)'
if cond(cov(:,:,cm))>cond_num
continue
else
break
end
end
end
% iteration to find local maxima
break_flag=0
while 1
a_old= a
mu_old= mu
cov_old=cov
fprintf(1,'calculating probability pmx...\n')
pause(0)
% pmx(m,x|param)
pmx=zeros(M,N)
for cm=1:M
cov_cm=cov(:,:,cm)
if cond(cov_cm) >cond_num
break_flag=1
end
inv_cov_cm=inv(cov_cm)
mu_cm=mu(:,cm)
for cn=1:N
%p_cm=exp(-0.5*(x(:,cn)-mu_cm)'*inv_cov_cm*(x(:,cn)-mu_cm))
p_cm=a(cm,:)*exp(-0.5*(x(:,cn)-mu_cm)'*inv_cov_cm*(x(:,cn)-mu_cm))%这里加上a
pmx(cm,cn)=p_cm
end
pmx(cm,:)=pmx(cm,:)/sqrt(det(cov_cm))
end
pmx=pmx*(2*pi)^(-K/2)
fprintf(1,'calculating conditional probability, p...\n')
pause(0)
% conditional probability p(m|x,param) for estimated parameters
p=pmx./kron(ones(M,1),sum(pmx))
fprintf(1,'updating parametres\n')
pause(0)
a = 1/N*sum(p')'
mu = 1/N*x*p'*diag(1./a)
for cm=1:M
a_cm=a(cm)
mu_cm=mu(:,cm)
tmp=x-kron(ones(1,N),mu_cm)
cov(:,:,cm)=1/N*(kron(ones(K,1),p(cm,:)).*tmp)*tmp'*diag(1./a_cm)
end
t=max([norm(a_old(:)-a(:))/norm(a_old(:))
norm(mu_old(:)-mu(:))/norm(mu_old(:))
norm(cov_old(:)-cov(:))/norm(cov_old(:))])
if print_flag==1
fprintf('c=%04d: t=%f\n',c,t)
c=c+1
end
if t<th
break
end
if c>Nit
disp('reach maximal iteration')
break
end
if break_flag==1
disp('***** break on singular covariance matrix *****')
break
end
end
f=sum(log(sum(pmx.*kron(ones(1,N),a))))
if f>f_best
a_best=a
mu_best=mu
cov_best=cov
f_best=f
end
end
!echo 真实的rand_num_a
rand_num_a
!echo 真实的rand_num_b
rand_num_b-rand_num_a
!echo 真实的rand_num_c
1-rand_num_b
!echo 迭代出来的a_best
a_best
!echo 真实的均值矩阵
mu_real
!echo 迭代出来的均值矩阵
mu_best
for cs=1:M
!echo 真实的协方差矩阵
cov_real(:,:,cs)
!echo 迭代出来的协方差矩阵
cov_best(:,:,cs)
end
%% plot all
% for 2D (K=2) only
x1_vect=-1:0.02:1
x2_vect=-1:0.02:1
px=zeros(length(x1_vect), length(x2_vect))
for c1=1:length(x1_vect)
for c2=1:length(x2_vect)
for cm=1:3
cov_real_cm=cov_real(:,:,cm)
mu_real_cm=mu_real(:,cm)
a_real_cm=a_real(cm)
x_cm=[x1_vect(c1)
x2_vect(c2)]
pm=a_real_cm*(2*pi)^(-0.5*K)*det(cov_real_cm)^(-0.5)*exp(-0.5*x_cm'*inv(cov_real_cm)*x_cm)
px(c1,c2)=px(c1,c2)+pm
end
end
end
px_hat=zeros(length(x1_vect), length(x2_vect))
for c1=1:length(x1_vect)
for c2=1:length(x2_vect)
for cm=1:3
cov_cm=cov(:,:,cm)
mu_cm=mu(:,cm)
a_cm=a(cm)
x_cm=[x1_vect(c1)
x2_vect(c2)]
pm=a_cm*(2*pi)^(-0.5*K)*det(cov_cm)^(-0.5)*exp(-0.5*x_cm'*inv(cov_cm)*x_cm)
px_hat(c1,c2)=px_hat(c1,c2)+pm
end
end
end
figure(1)clfhold on
hold on
title('理论图')
xlabel('x')
ylabel('y')
mesh(x1_vect, x2_vect, px)
figure(2)clfhold on
mesh(x1_vect, x2_vect, px_hat)
title('统计图')
xlabel('x')
ylabel('y')
function 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 %目标1的状态噪声协方差阵
Q2=4000 %目标1的状态噪声协方差阵
R=10000 %观测噪声协方差阵
%R2=700
x00_1=[0 0 0]' %模型1估计的初始值
x00_2=[0 0 0]' %模型2估计的初始值
p00_1=[100 00
0 10 0
0 0 0]
p00_2=[100 00
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
我用matlab53版本执行此程序，没有发现有什么问题，程序可以执行。如果你嫌中间输出多的话，可以if rand(1)<0.4
a(j)=random('Normal',0,1,1,1)
else
a(j)=random('Normal',2,2,1,1)
end
加上 如下所示：
if rand(1)<0.4
a(j)=random('Normal',0,1,1,1)
else
a(j)=random('Normal',2,2,1,1)
end
你的程序中没有把计算结果输出。如果你想要知道计算结果，比如上面的z1
你在命令窗口中输入？z1
就可以了
如果你想保存 z1 z2，用下述命令就可以了：
save z1z2dat.mat z1 z2
就可以了
或者再次使用这两个量时，用：
?load z1z2dat.mat
?z1
就可以了。
欢迎分享，转载请注明来源：内存溢出
微信扫一扫
支付宝扫一扫
评论列表（0条）