equivalent_weights_filter_zhu.f90

Go to the documentation of this file.

!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!!! Time-stamp: <2016-10-18 15:03:15 pbrowne>
!!!
!!!    Computes the equal weights step in the New Schem Equal Weights Particle Filter
!!!    Copyright (C) 2015  Mengbin Zhu
!!!
!!!    This program is free software: you can redistribute it and/or modify
!!!    it under the terms of the GNU General Public License as published by
!!!    the Free Software Foundation, either version 3 of the License, or
!!!    (at your option) any later version.
!!!
!!!    This program is distributed in the hope that it will be useful,
!!!    but WITHOUT ANY WARRANTY; without even the implied warranty of
!!!    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
!!!    GNU General Public License for more details.
!!!
!!!    You should have received a copy of the GNU General Public License
!!!    along with this program.  If not, see <http://www.gnu.org/licenses/>.
!!!
!!!    Email: p.browne @ reading.ac.uk  EMPIRE
!!!           zhumengbin@gmail.com      new scheme equal weights particle filter
!!!    Mail:  School of Mathematical and Physical Sciences,
!!!           University of Reading,
!!!       Reading, UK
!!!       RG6 6BB
!!!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
subroutine equivalent_weights_filter_zhu
  use output_empire, only : emp_e
  use timestep_data
  use pf_control
  use sizes
  use random
  use comms
  implicit none
  include 'mpif.h'
  integer, parameter                             :: rk = kind(1.0d0)
  real(kind=rk), dimension(pf%count)             :: alpha,c
  real(kind=rk), dimension(pf%count)             :: epsiloni,epsilon_n,epsilon_p,gammai,ce,win,wsolve
  real(kind=rk), dimension(pf%nens)              :: csorted
  real(kind=rk), dimension(pf%nens)              :: weight_an
  real(kind=rk)                                  :: cmax,xs1,xs2
  integer                                        :: particle,i,mpi_err,rc!,tag
  real(kind=rk)                                  :: percentage,randc
  real(kind=rk), dimension(obs_dim)              :: y     !y, !//!<the observations
  real(kind=rk), dimension(obs_dim,pf%count)     :: Hfpsi           !H(f(psi^(n-1))) !< \f$H(f(x^{n-1}))\f$
  real(kind=rk), dimension(obs_dim,pf%count)     :: y_Hfpsin1  !y-H(f(psi^(n-1))) !< \f$y-H(f(x^{n-1}))\f$
  real(kind=rk), dimension(state_dim,pf%count)   :: fpsi     !f(psi^(n-1)) !< \f$f(x^{n-1})\f$
  real(kind=rk), dimension(state_dim)            :: psimean
  real(kind=rk), dimension(state_dim,pf%count)   :: kgain !QH^T(HQH^T+R)^(-1)(y-H(f(psi^(n-1)))) !< \f$QH^T(HQH^T+R)^{-1}(y-H(f(x^{n-1})))\f$
  real(kind=rk), dimension(state_dim,pf%count)   :: betan         !the random variable
  real(kind=rk), dimension(state_dim,pf%count)   :: statev
  real(kind=rk)                                  :: w

  real(kind=rk), parameter                       :: pi = 4.0D0*atan(1.0d0)

  real(kind=rk), dimension(pf%count)             :: weight_temp2
  real(kind=rk)                                  :: ddot

  integer :: ensemble_comm
  real(kind=rk) :: gammaitemp

  percentage = 0.5
  rc=1



  if(comm_version .eq. 1 .or. comm_version .eq. 2) then
     ensemble_comm = pf_mpi_comm
  elseif(comm_version .eq. 3) then
     ensemble_comm = pf_ens_comm
  else
     write(emp_e,*) 'EMPIRE VERSION ',comm_version,' NOT SUPPORTED IN propos&
          &al_filter'
     write(emp_e,*) 'THIS IS AN ERROR. STOPPING'
     stop '-24'
  end if

  !start the model integration
  call send_all_models(state_dim,pf%count,pf%psi,1)

  !get the next observation and store it in vector y
  call get_observation_data(y,pf%timestep)

  !recv the models after integration
  call recv_all_models(state_dim,pf%count,fpsi)

  call h(obs_dim,pf%count,fpsi,hfpsi,pf%timestep)

  !compute c for each particle on this mpi thread
  do i = 1,pf%count
     particle = pf%particles(i)
     y_hfpsin1(:,i) = y - hfpsi(:,i)

     call innerhqht_plus_r_1(y_hfpsin1(:,i),w,pf%timestep)

     c(i) = 2.0*pf%weight(particle) + w
  end do


  !communicate c to all the mpi threads
  call mpi_allgatherv(c,pf%count,mpi_double_precision,csorted,gblcount&
       &,gbldisp,mpi_double_precision,ensemble_comm,mpi_err)

  !calculate cmax
  call quicksort_d(csorted,pf%nens)
  cmax = csorted(pf%nens)


  psimean = 0.0_rk


  !compute the kalman gain
  call k(y_hfpsin1,kgain)

  !generate the unit normal random numbers for all particles ~ xi
  call normalrandomnumbers2d(0.0d0,1.0d0,state_dim,pf%count,statev)

  !calculate epsiloni for the particles
  xs1=-0.90d0
  xs2=2.1d0
  do i = 1,pf%count
     gammai(i) = ddot(state_dim,statev(:,i),1,statev(:,i),1)
     if(comm_version .eq. 3) then
        !need to perform the sum across all parts of the state vector          
        gammaitemp=gammai(i)
        call mpi_allreduce(gammaitemp,gammai(i),1,mpi_double_precision,mpi_sum&
             &,pf_member_comm,mpi_err)
     end if


     ce(i) = cmax - c(i)

     win(i) = (-gammai(i)/state_dim_g)*exp(-gammai(i)/state_dim_g)*exp(-ce(i)/state_dim_g)
     call lambertw(0,win(i),wsolve(i))
     if(ce(i) .eq. 0.0) then
        wsolve(i) = -(gammai(i)/state_dim_g)
     end if
     epsilon_n(i) = (-state_dim_g/gammai(i))*wsolve(i) - 1

     !call newton(xs1,gammai(i),state_dim,c(i),cmax,epsilon_n(i))

     call lambertw(-1,win(i),wsolve(i))
     epsilon_p(i) = (-state_dim_g/gammai(i))*wsolve(i) - 1

     !call newton(xs2,gammai(i),state_dim,c(i),cmax,epsilon_p(i))
  end do

  do i = 1,pf%count
     if(comm_version .eq. 1 .or. comm_version .eq. 2) then
        call random_number(randc)
     else
        if(pf_member_rank .eq. 0) then
           call random_number(randc)
        end if
        call mpi_scatter(randc,1,mpi_double_precision,randc,1&
             &,mpi_double_precision,0,pf_member_rank,mpi_err)
     end if
     if(randc .ge. 0.5) then
        epsiloni(i) = epsilon_p(i)
     else
        epsiloni(i) = epsilon_n(i)
     end if
  end do


  !compute alpha for each particle on this mpi thread
  do i = 1,pf%count
     alpha(i) = 1.0 + epsiloni(i)
  end do

  !compute the random term of the analysis 
  call phalf(pf%count,statev,betan)

  !update the weights and the new state
  do i = 1,pf%count
     particle = pf%particles(i)
     weight_an(particle) = 0.0
     if(alpha(i) .eq. 0.0) then
        alpha(i) = alpha(i) + 1.0e-16
     end if
     weight_an(particle) = gammai(i)*epsiloni(i)-state_dim_g*log(alpha(i))+c(i)
     weight_an(particle) = 0.5*weight_an(particle)
     if(alpha(i) .eq. 1.0e-16) then
        weight_an(particle) = 0.5*cmax
     end if
     pf%Weight(particle) = weight_an(particle)

     !now do the following
     !x^n = f(x^(n-1)) + K (y-Hf(x_i^n-1)) + alpha(i)^(1/2) P^(1/2) xi
     call update_state(pf%psi(:,i),fpsi(:,i),kgain(:,i),sqrt(alpha(i))*betan(:,i))
     psimean = psimean + pf%psi(:,i)
  end do

  !store in weight_temp2 only those weights on this mpi thread
  weight_temp2 = -huge(1.0d0)
  do i = 1,pf%count
     weight_temp2(i) = pf%weight(pf%particles(i))
  end do

  !communicate the weights of all particles to each mpi thread
  call mpi_allgatherv(weight_temp2,pf%count,mpi_double_precision,pf%weight,gblcount&
       &,gbldisp,mpi_double_precision,ensemble_comm,mpi_err)

  !normalise the weights
  pf%weight = exp(-pf%weight+maxval(pf%weight))
  pf%weight = pf%weight/sum(pf%weight)
  pf%weight = -log(pf%weight)
  !=========================================================================


  call timestep_data_set_is_analysis
  if(pf%use_talagrand) call diagnostics

end subroutine equivalent_weights_filter_zhu