A little background.
You need to weight each particle by the liklihood of the particle being correct. The probability the particle is correct is given by the probability that it is correct given the measurements.
- Note that the "weight" (which is a terrible term) is simply the probability of the particle being correct. Therefore, each particle is really an estimate, and each estimate has some probability of being correct.
- Let $x$ be the state. There are a bunch of (say $N$) state estimates ($x_i$ for $i=1...N$), i.e., particles
- Let $z$ be your measurement
By Bayes rule:
$$p(x|z) = \frac{p(z|x)\cdot p(x)}{p(z)}$$
Which means, the probability of the particle being "correct" given the measurement is given by the probability that the measurement "makes sense" given the particle's estimate, times the probability that the particle was correct to begin with.
The denominator is commonly written using the law of total probability so the above becomes:
$$p(x_i|z) = \frac{p(z|x_i)\cdot p(x_i)}{\sum_{j=1}^N p(z|x_j)p(x_j)}$$
You can re-weight each particle using the above. Explicitly,
$$w_i\gets \frac{p(z|x_i)\cdot w_i}{\sum_{j=1}^N p(z|x_j)\cdot w_j}$$
Now, let's go over your problem.
You said you have a tuple $(x_1,y_1,x_2,y_2)$, but that's not correct, you have two measurements of the $(x,y)$ value for the robot's position. You need to do two updates of each particle. (EDIT: It seems the three-dimensional state of the robot was derived from these two points. I misunderstood so ignore that)
So, first calculate $p(z|x_i)$ for each particle $x_i$. In a two dimensional state, this is done as follows:
- for all $x_i$, calculate $p_i \gets p(z|x_i)$
E.G., In matlab: pi=chi2pdf( norm(z-xi), 2))
. See: chi2pdf and norm. You may want to see this question if you don't understand this step.
- Now, let $D\gets\sum_{i=1}^N p(z|x_i)\cdot w_i$ using the above values of $p_i$ and previous values of $w_i$ for each particle.
- Then, let the new value of each particle be: $w_i\gets\frac{p_i\cdot w_i}{D}$.
For a complex discussion of why this works, what about it isn't so great, and so on, you'll want to read some literature. Thrun's Probablistic Robotics is a great resource.
EDIT: I should mention that commonly $p(z|x_i)$ is calculated not using CHI2PDF but instead by the EKF linearization of $z=h(x_i)$. That's beyond scope. Furthermore, the uncertainty in the robot position is not included. Using an EKF for each particle will address this but that's a different framework (sort of).