Adding measuruments noises to kalman filter

Question

I'm implementing navigation system for my robot. There are two ways to get data from it: odometry(encoders from motors) and camera. Both of information sources can give me estimate of robot's position. But camera has very big noise. I've found that it is useful to use Kalman filter. After some literature looking I have a questions:

1. How to use kalman filter with adding measurement noise $v(k)$? $$x(k+1) = F(k)x(k-1)+B(k)u(k)+w(k)$$ $$ z(k) = H(k)x(k)+v(k)$$

2. How to make "coefficient of trust" for camera measurements in Kalman gain very low (because of very very high camera noise)? (Probably is it needed to make specific Q or R matrix?)

I would appreciate any help. Thanks.

Answer 1

For the first question, in Kalman filter there are two noises namely process and observation noises. Kalman filter assumes the noise to be Gaussian with zero mean and some variance $\mathcal{N}(0, \sigma^{2})$. Let's say we have a sensor that measures a constant quantity but random, for example distance. Let's say $x = 1$m, however the sensor gives us $z=0.9$m. In reality we don't know $x$ for certain so we rely on our sensor. In Matlab, we can simulate this sensor

x = 1; % true value
z = x + randn(); % observed value

randn() is normal distribution with zero mean and variance one $\mathcal{N}(0, 1)$. If you want to increase the accuracy of your sensor, then multiply the normal distribution randn() with some small value that you think it is the error of your sensor.

for i = 1:6
    x = 1; % true value
    z = x + 0.01*randn() % observed value
end

You will notice the observed value varies from the true value however still close to the true value because of 0.01. Remember that Kalman filter is a matter of modeling your system accurately. For kalman filter, you need the motion and observation models given in advanced. Also, your system must be linear and if this is not the case then you should use Extended kalman filter. Moreover, the noise must be Gaussian noise. Now your job to determine these parameters.