def PF_AUS(truth, obs, R,initial_cloud,bred_perts,u,tfin=2000,tanl=20): """Returns [mean_state, mean_error,avg_mean_error,rsn] for mandatory inputs truth trajectory, observations, and intitial cloud. [state_dimension, time_step] = shape(truth) [state_dimension, Nanl] = shape(obs) [state_dimension, state_dimension] = shape(R) [state_dimension, particle_number] = shape(initial_cloud) scalar = type(u) [state_dimension, Nanl] = shape(bred_perts) [state_dimension, time_step] = shape(mean_state) [state_dimension, time_step] = shape(mean_error) [time_step] = shape(avg_mean_error) # of times resampler triggered = rsn Optional arguments include the time step=h=0.01, sigma=a=10, beta=8/3, rho=29, tanl=20. The duration of the experiment tfin=2000, and analysis time particle cloud is propagated between analysis times, at which point the observation is incorporated through the BAUS update and the Bayesian update of the weights. Resampling is initiated according to resamp_gauss when Neff is less than the threshold.""" # NUMBER OF ANALYSES Nanl = int(tfin/tanl) #number of resample steps rsn = 0 #number of failsafe steps fsn = 0 #Set initial trajectory matricies and weights [state_dimension, particle_number] = shape(initial_cloud) part = zeros([state_dimension,tfin+1,particle_number]) part[:,0,:] = initial_cloud part_weights = ones(particle_number)*(1.0/particle_number) Q = inv(R) mean_state = zeros([state_dimension,tfin+1]) #corrections = zeros([3,Nanl]) #loop over number of analyses for j in range(Nanl): #propagate each particle to next analysis step part[:,j*tanl:(j+1)*tanl+1,:] = IMV(tanl,part[:,j*tanl,:],u) #Calculate Mean at each propagated state up to analysis time step weighted_cloud = part[:,j*tanl:(j+1)*tanl,:]*part_weights mean_state[:,j*tanl:(j+1)*tanl]=mean(weighted_cloud,2)*particle_number #Bayesian update is applied to particle wieghts innov = obs[:,j] - part[:,(j+1)*tanl,:].T temp = array([sum(innov*Q[:,0],1), sum(innov*Q[:,1],1)]) part_weights = part_weights*exp(-0.5*sum(temp*innov.T,0)) part_weights = part_weights/sum(part_weights) #Resampling step if number of effective samples falls below threshold n_eff = 1/(part_weights.dot(part_weights)) if (n_eff < 2) or isnan(n_eff): if isnan(n_eff): fsn = fsn + 1 part_weights = ones(particle_number)*(1.0/particle_number) rsn = rsn + 1 part[:,(j+1)*tanl,:] = resamp_gauss(part[:,(j+1)*tanl,:], part_weights,.0004, .01/particle_number) part_weights = ones(particle_number)*(1.0/particle_number) #Find mean state updated weights weighted_cloud = part[:,(j+1)*tanl,:]*part_weights mean_state[:,(j+1)*tanl] = mean(weighted_cloud,1)*particle_number #Lyapunov Mode supplied from the truth trajectory L = bred_perts[:,j] #Calculate the innovation for each particle innov = obs[:,j] - part[:,(j+1)*tanl,:].T #project into the unstable subspace proj = sum(innov*L,1) L = (L*ones([particle_number,state_dimension])).T proj = proj*L #correct the particle again with AUS part[:,(j+1)*tanl,:] = part[:,(j+1)*tanl,:] + proj print(fsn) print(rsn - fsn) #mean_error_av = mean_error*mean_error mean_error = abs(mean_state - truth) error_dist = zeros(tfin+1) avg_mean_error = zeros(tfin+1) for i in range(tfin+1): error_dist[i] = sqrt(mean_error[:,i].dot(mean_error[:,i])) avg_mean_error[i] = mean(error_dist[0:i+1]) return(mean_state,mean_error, avg_mean_error)
def PF(truth, obs, R,initial_cloud,h): """Returns [part, mean_state, mean_error,avg_mean_error] for mandatory inputs truth trajectory, observations, and intitial cloud. [state_dimension, tfin+1] = shape(truth) [state_dimension, Nanl] = shape(obs) [state_dimension, state_dimension] = shape(R) [state_dimension, particle_number] = shape(initial_cloud) [state_dimension, Nanl] = shape(bred_perts) [state_dimension, time_step] = shape(mean_state) [state_dimension, time_step] = shape(mean_error) [time_step] = shape(avg_mean_error) # of times resampler triggered = rsn Optional arguments include sigma=a=10, beta=8/3, rho=29, tanl=20. The duration of the experiment tfin=2000, and analysis time particle cloud is propagated between analysis times, at which point the observation is incorporated through the Bayesian update of the weights. Resampling is initiated according to resamp_gauss when Neff is less than the threshold.""" #Recover the integration length tfin = int(len(truth[0,:])-1) # NUMBER OF ANALYSES Nanl = len(obs[0,:]) #Recover the time between analyses tanl = int(tfin/Nanl) #Set initial trajectory matricies and weights [state_dimension, particle_number] = shape(initial_cloud) part = zeros([state_dimension,tfin+1,particle_number]) part[:,0,:] = initial_cloud part_weights = ones(particle_number)*(1.0/particle_number) Q = inv(R) mean_state = zeros([state_dimension,tfin+1]) """ figure() subplot(projection='3d') plot(part[0,0,:], part[1,0,:], part[2,0,:],'y.') show() """ #corrections = zeros([3,Nanl]) #loop over number of analyses for j in range(Nanl): #propagate each particle to next analysis step part[:,j*tanl:(j+1)*tanl+1,:] = EL63V(tanl,h,part[:,j*tanl,:],a,r,b) """ for jj in range(tanl): figure() subplot(projection='3d') plot(part[0,j*tanl + jj,:], part[1,j*tanl + jj,:], part[2,j*tanl + jj,:],'r.') plot([truth[0,j*tanl+jj]], [truth[1,j*tanl+jj]], [truth[2,j*tanl+jj]],'bo') show() print('analysis time' + str(j)) """ #Calculate Mean at each propagated state up to analysis time step weighted_cloud = part[:,j*tanl:(j+1)*tanl,:]*part_weights mean_state[:,j*tanl:(j+1)*tanl]=mean(weighted_cloud,2)*particle_number #Bayesian update is applied to particle wieghts innov = obs[:,j] - part[:,(j+1)*tanl,:].T temp = array([sum(innov*Q[:,0],1), sum(innov*Q[:,1],1), sum(innov*Q[:,2],1)]) w_temp = part_weights*exp(-0.5*sum(temp*innov.T,0))**(1.0/3.0) if sum(w_temp)<exp(-9): w_temp = ones([particle_number])*(1.0/particle_number) else: w_temp = w_temp/sum(w_temp) part_weights = part_weights*w_temp part_weights = part_weights/sum(part_weights) #effective number of samples computed n_eff = 1/(part_weights.dot(part_weights)) #Resampling step if number of effective samples falls below threshold if (n_eff < particle_number/2): part[:,(j+1)*tanl,:] = resamp_gauss(part[:,(j+1)*tanl,:], part_weights,.0004, .1/particle_number) part_weights = ones(particle_number)*(1.0/particle_number) #Find mean state updated weights weighted_cloud = part[:,(j+1)*tanl,:]*part_weights mean_state[:,(j+1)*tanl] = mean(weighted_cloud,1)*particle_number """ figure() subplot(projection='3d') plot(part[0,(j+1)*tanl,:], part[1,(j+1)*tanl,:], part[2,(j+1)*tanl,:],'y.') plot([obs[0,j]],[obs[1,j]],[obs[2,j]],'bo') show() """ #mean_error_av = mean_error*mean_error mean_error = abs(mean_state - truth) error_dist = sqrt(sum(mean_error*mean_error,0)) r_avg_mean_error = zeros(tfin+1) print('PF') for i in range(tfin+1): r_avg_mean_error[i] = mean(error_dist[0:i+1]) return(part, mean_state, mean_error, r_avg_mean_error)