// Update belief state with new particles
    void UpdateBelief()
    {
        // Get observation from Lidar and action(displacement) of the agent
        float[] agent_observation = _lidar_script.GetDistanceObservation();
        Vector3 action            = GetAction();

        // Create _weights for each sampled particle
        _weights = new float[_beliefStates.Length];
        Vector3[] sampledPositions = new Vector3[_beliefStates.Length];

        bool has_high_confidence = false;

        // Sample from belief states
        for (int i = 0; i < _beliefStates.Length; i++)
        {
            int        randomParticleIndex = Random.Range(0, _beliefStates.Length);
            GameObject randomParticle      = _beliefStates[randomParticleIndex];
            // Update the sampled particle's position by taking the action performed by the agent
            Vector3 newParticlePosition = randomParticle.transform.position + action;
            // sampledPositions[i] = randomParticle.transform.position;
            sampledPositions[i] = newParticlePosition;
            float[] particle_observation = _lidar_script.Scan(newParticlePosition + new Vector3(0, 0, 2));

            int numRayMatches           = 0;
            int numRayObserved_agent    = 0;
            int numRayObserved_particle = 0;
            // Compare particle observation with agent observation
            for (int j = 0; j < agent_observation.Length; j++)
            {
                // If the observation is positive and within threshold then increment match counts
                if ((agent_observation[j] > 0 && particle_observation[j] > 0 &&
                     Mathf.Abs(agent_observation[j] - particle_observation[j]) < observation_threshold))
                {
                    numRayMatches++;
                }
                // Count how many rays are observed by the agent
                if (agent_observation[j] != -1)
                {
                    numRayObserved_agent++;
                }
                // Count how many rays are observed by the particle
                if (particle_observation[j] != -1)
                {
                    numRayObserved_particle++;
                }
            }
            // Heavily pentalize particle weight if the particle observed more than the agent
            if (numRayObserved_particle > numRayObserved_agent)
            {
                numRayObserved_agent = 2 * Mathf.Max(numRayObserved_agent, numRayObserved_particle);
            }
            // Weight is the percentage of rays that matched between agent and particle observations
            _weights[i] = (float)(numRayMatches + 1) / (numRayObserved_agent + 1);
            if (_weights[i] > upper_confidence_threshold)
            {
                has_high_confidence = true;
            }
        }


        // If there's at least one particle with high confidence or all particles are low confidence, keep noise level normal. Otherwise increase noise.
        // Noise is increased when there are observations but none of the particles match the observations.
        float multiplier = has_high_confidence ? 1 : noise_multiplier;

        // Resample using the _weights and update the belief
        for (int i = 0; i < _beliefStates.Length; i++)
        {
            int     randomIndex = WeightedRandomIndex(ref _weights);
            Vector3 noise       = new Vector3(SampleNormal(NOISE_MEAN, multiplier * NOISE_STD), SampleNormal(NOISE_MEAN, multiplier * NOISE_STD), 0);
            _beliefStates[i].transform.position = sampledPositions[randomIndex] + noise;
        }

        //Update beliefs with information from other agents
        if (evalAdvanced)
        {
            MultiAgentUpdate();
        }
    }