- #1
bolly
- 16
- 2
Dear Community,
I want to use a Kalman- filter approach to calculate inclination angles (pitch and roll) obtained from accelerometer- and gyroscope- raw data. However the problem is that my sampling interval dt is not constant – it has a significant jitter instead. To investigate whether the kalman filter can handle such data I’ve experimented using the following MATLAB code copied from here (http://web.csulb.edu/~hill/ee400d/Project%20Folder/Kalman%20Filter%20Research.pdf):
I’ve found that with increasing bandwidth of dt the filter refuses to work. As far I’ve understood the kalman approach can be applied to zero the linear integration-error obtained by integrating the angular velocity measured using a gyroscope (if the this error propagtes linear).
However can it also be applied to zero the sampling error caused by a jitter of dt ?
I would be very happy if someone could give a hint why the kalman approach refuses to work with increasing bandwidth of dt.
I want to use a Kalman- filter approach to calculate inclination angles (pitch and roll) obtained from accelerometer- and gyroscope- raw data. However the problem is that my sampling interval dt is not constant – it has a significant jitter instead. To investigate whether the kalman filter can handle such data I’ve experimented using the following MATLAB code copied from here (http://web.csulb.edu/~hill/ee400d/Project%20Folder/Kalman%20Filter%20Research.pdf):
Code:
stacksize = 100;dt_stack = rand(stacksize,1)/2;
t_stack = zeros(stacksize,1);
t_stack(1) = 0;
for count=2:stacksize
t_stack(count) = t_stack(count-1) + dt_stack(count);
end
x = sin(t_stack); % angle
x_dot = cos(t_stack); % angular velocity
figure;
subplot(3,1,1);
plot(t_stack);
subplot(3,1,2);
plot(t_stack,x);
hold on;
plot(t_stack,x_dot, 'Color','red');
P = [1 0; 0 1];R_angle = 0.9;
Q_angle = 0.05;
Q_gyro = 0.5;
Q = [Q_angle 0; 0 Q_gyro];
A = [0 -1; 0 0];
q_bias = 0; % Initialize gyro bias
angle = 0; % Initialize gyro angle
q_m = 0;
X = [0; 0];
x1 = zeros(stacksize);
x2 = zeros(stacksize);
H = [1 0];
x_calc = 0;
x_dot_corr = 0;
gyro_bias = 0;
angle_err = 0;
for i=1:100
x_dot_corr = x_dot(i) - gyro_bias; % /* Pitch gyro measurement */
x_calc = x_calc + x_dot_corr*dt_stack(i);
x_err = x(i)- x_calc;
Pdot = A*P + P*A' + Q;
P = P + Pdot*dt_stack(i);
E = H*P*H' + R_angle;
K = P*H'*inv(E);
P = P - K*H*P;
X = X + K * x_err;
x1(i) = X(1);
x2(i) = X(2);
x_calc= x1(i);
gyro_bias = x2(i);
end
x = x(1:stacksize);
x1 = x1(1:stacksize);
length(x)
length(x1)
length(t_stack)% Plot the result using kalman filter
subplot(3,1,3);
plot(t_stack,x);
hold on
plot(t_stack,x1,'Color','green');
xlabel('time(s)');
ylabel('x(t)');
legend('actual','kalman');
However can it also be applied to zero the sampling error caused by a jitter of dt ?
I would be very happy if someone could give a hint why the kalman approach refuses to work with increasing bandwidth of dt.
Last edited: