Hi just made a Kalman filter in Matlab :P

function uav() dt = 0.1; duration = 10; a = [1 -dt; 0 1]; % transition matrix b = [dt; 0]; % input matrix c = [1 0]; % measurement matrix x = [0; 0]; % initial state vector X = [ angle, bias ] P = [1 0; 0 1]; R = 0.1; % measurment covariance noise {R=0 Perfect follow} Q = [0.001 0; 0 0.003]; % process covariance noise (Q_angle = 0.001 Q_gyro = 0.003) x_noise = 0:dt:duration; n = 1; mag = []; vig = []; for t = 0 : dt: duration %y_noise(n) = x_noise(n)*x_noise(n)-2*x_noise(n); %za_noise(n)= (x_noise(n)*x_noise(n)-2*x_noise(n))+(2*rand-1)*0.2; %zg_noise(n)=(x_noise(n)*x_noise(n)-2*x_noise(n))+(2*rand-1)*0.2; y_noise(n) = sin(x_noise(n)); za_noise(n)= sin(x_noise(n))+(2*rand-1)*0.2; zg_noise(n)= sin(x_noise(n))+(2*rand-1)*0.2; u = za_noise(n); y = zg_noise(n); x = a*x + b*u; P = a*P*a' + Q; Inn = y - c*x; % accelerometer S = c*P*c' + R; K = a*P*c'*inv(S); x = x + K*Inn; P = a*P*a'-a*P*c'*inv(S)*c*P*a mag=[mag;x(1)]; vig=[vig;x(2)]; n=n+1; end %plot(x_noise,y_noise, x_noise,za_noise, x_noise, zg_noise) %plot(x_noise,za_noise,'b', x_noise,zg_noise,'b', x_noise, mag,'r');%, x_noise, vig,'g') plot(x_noise,y_noise,'b', x_noise, mag,'r')

Does anybody have experience with Kalmanfiltering? Because I'm not impressed with the accuracy of the filter I made, and are woundering if I have made any mistakes?? :?

Magnus T