I have a quite typical Kalman filter to design. I really read a lot of articles about the design of this filter but the performances of my filter are still quite bad.
Here is my situation. I have a quite good measurement signal of my position (let's say a very small white noise) and a pretty noisy measurement signal of my velocity (big white noise). I want to estimate a new position (which will be I guess not very different from my measurements) and my velocity as well (which will be more different)
Here is my Matlab code: (I don't want to use the Matlab Kalman function ;) )
%% Kalman Filter Design dt = 0.01; %% Define coefficient matrices % x = [position;velocity;acceleration] % True State equation % x = F * x(t-1) + B * u + w F = [ 1 dt dt^2/2 0 1 dt 0 0 1]; B = [0 0 0]; % Measurement of the true state equation % z = H * x(t) + v H = [1 0 0 0 1 0]; %% Define noise %Process noise(white noise) % w = Gw * a a = [0 ; 0 ; wgn(1,1,100)]; Gw = [0 0 0 0 0 0 0 0 1]; %Process noise covariance matrix Q = Gw * Gw' * cov(a); %Measurement noise (white noise) v = [wgn(1,1,0.001) ; wgn(1,1,20)]; %Measurement noise covariance matrix R = cov(v)* [0.5 0 0 1]; %% Kalman Filter x_estimate = [0;0;0]; P = Q; position_estimate = ; velocity_estimate = ; acc_estimate = ; P_mag_estimate = ; predic_state =; predic_var = ; z = [position_meas,velocity_meas]; for t = 1:length (z) %Predicted state estimate x_estimate = F * x_estimate; predic_state = [predic_state; x_estimate(1)]; %Predicted estimate covariance P = F * P * F' + Q; predic_var = [predic_var; P]; %Innovation covariance S = H * P * H' + R ; %Kalman Gain Predict measurement covariance K = P * H' * inv(S); % Updated state estimate x_estimate = x_estimate + K * (z(t) - H * x_estimate); %Update covariance estimation P = (eye(size(P,2)) - K * H) * P; %Store for plotting position_estimate = [position_estimate; x_estimate(1)]; velocity_estimate = [velocity_estimate; x_estimate(2)]; P_mag_estimate = [P_mag_estimate; P(1)]; end
My question is: how can I find Q and R? Does it depend of what I want to do?
Thanks a lot for your help