Likelihood for First order censored autoregressive process with covariate(see Jung Wook Park1 , Marc G. Genton2 and Sujit K. Ghosh3)

Question

library(mvtnorm)

set.seed(14)
n=10000 
sigmatrue<-1
 rhotrue<-0.3
  b1=0.05
  b0=0
   y<-arima.sim(model=list(ar=c(0.3)),n=10000 ,sd=sigmatrue)#kataskevi 
    #xronoseiras

 x=rep(0,n)

for(i in 1:n){
  x[i]=i
}
for(t in 1:n)
{   
  y[t]=y[t]+b0+b1*x[t]
}
est=arima(y,order=c(1,0,0),xreg=x,include.mean=TRUE,method="ML",kappa=1e+06)
cens<-rep(0, n)
c=(9/10)*(n*b1+b0)
for (i in 1:n) {
  if(y[i]>c){
    y[i]<-c
    cens[i]<-1
  }
}

  ll<-function(p){
    sigma=matrix(c(p[2]^2/(1-p[3]^2), p[2]^2*p[3]/(1-p[3]^2),p[2]^2*p[3]/(1-p[3]^2),p[2]^2/(1-p[3]^2)),ncol=2,nrow=2,byrow=TRUE)
    likelihood<-rep(0,n)
    for(t in 2 :n){
      if(cens[t]==0 & cens[t-1]==0){
        likelihood[t]<-dnorm(((y[t]-(p[1]+p[4]*t)-p[3]*(y[t-1]-(p[1]+p[4]*(t-1)))/p[2]) )/p[2])
      }
      else if(cens[t]==0 & cens[t-1]==1){
        likelihood[t]<-(1/(1-pnorm((c-(p[1]+p[4]*t)*sqrt(1-p[3]^2)/p[2]))*sqrt(1-p[3]^2)/p[2]*dnorm(((y[t]-(p[1]+p[4]*t)*sqrt(1-p[3]^2))/p[2])*(1-pnorm(((c-(p[1]+p[4]*(t))-p[3]*(y[t]-(p[1]+p[4]*(t-1)))/p[2])))))))
      }
      else if(cens[t]==1 & cens[t-1]==0){
        likelihood[t]<-1-pnorm(((c-(p[1]+p[4]*t)-p[3]*(y[t-1]-(p[1]+p[4]*(t-1)))/p[2])))
      }
      else
      {
        likelihood[t]<-(((pmvnorm(lower=c, upper=Inf , mean=c(p[1]+p[4]*(t-1),p[1]+p[4]*t),sigma=sigma))/(1-pnorm((c-(p[1]+p[4]*(t-1))*sqrt(1-p[3]^2)/p[2])))))
      }
    }


    f0=(sqrt(1-p[3])/p[2]*dnorm(((y[1]-p[1]-p[4])*sqrt(1-p[3]^2))/p[2]))

    likelihood[1]=f0
    #Ta prosthesa
    if (any(likelihood==0)){
      likelihood[likelihood==0] = 0.000001 #poly mikros arithmos
    }

    if (any(likelihood==Inf)){
      likelihood[likelihood==Inf] = 1 #poly megalos h 1, an milame gia pi8anothta
    }

    if (any(is.nan(likelihood))){
      likelihood[is.nan(likelihood)] = 0.000001
    }

    minusloglike=-sum(log(likelihood))
    #l1=list(Minusloglike=minusloglike,Loglikelihood=log(likelihood))
    return(minusloglike)
  }
  fit<-optim(c(0,1,0.3,0.05),ll,method="L-BFGS-B",lower=c(-Inf,0.001,-0.999,-Inf),upper = c(Inf,Inf,0.999,Inf),hessian=TRUE)
  fisher.info<-solve(fit$hessian)
  fisher.info
  prop.sigma<-sqrt(diag(fisher.info))
  sigmas<-diag(prop.sigma)
  upper<-fit$par+1.96*sigmas
  lower<-fit$par-1.96*sigmas
  interval<-data.frame(value=fit$par, lower=diag(lower),upper=diag(upper))
  interval

I run this code(it is for censored first order autogressive process with covariate , i have 4 cases for x(t) ,x(t-1) either is censored or non-censored and i dont want the likelihood to go near zero and inf).I get error

Error in if (any(likelihood == Inf)) { : missing value where TRUE/FALSE needed Called from: fn(par, ...)

The program is working for n=100 but when n is larger than 100 i have this error. I think this error causes bad estimattes of the four parameters(b1,rho,sigma,b0).Does anyone know what can i do?

Thank you for your help.