// 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();
}
}