In order for a robot to navigate autonomously it needs to know where it is relative to some point or points. This article focuses on an algorithm called Kalman filter which combines noisy sensors and a model of how the robot can and should move which might also be inaccurate to get a more accurate position estimate.
The Kalman filter is part of a large family of positioning and mapping algorithms called SLAM(Simultaneous Localization and Mapping). The basic idea behind SLAM is to use a LiDAR or camera to create and update a map(2D or 3D) in real time and at the same time use sensors like GPS(if outdoors), IMU and encoders to pinpoint the location and orientation of the robot within this map.
Sensors and their jargon:
Kalman filter(KF) is a predictor corrector linear estimator. This means it makes a prediction(using probability) based on a model(or a sensors input like IMU) and then corrects that prediction(using more probability) based on sensor input(like encoder or GPS input) to give us the final estimate. Then it repeats this process. Since it learns from its previous predictions and corrections it usually gets better over time. Since its a linear estimator, the model has to be linear or it has to be linearized(called extended Kalman filter)(by the way in this context linear implies having no bias for the Kalman filter inputs, if have bias then the bias is a state in the state matrix).
The linear model is always in the form Xnext=AX+BU and Y=HX+v where X is the state matrix containing the different state variables(ex: position and velocity), U is the input, y is the output, v is the measurement noise and A,B and H are proportionality matrices.
For correction there are once again 2 equations: one for the state matrix and one for the covariance matrix.
for the state matrix(Xhat):
Xhat=Xhat+K*(measurement-H*Xhat)
aka
Xhat=previous prediction(aka Xhat from prediction)+K *how wrong that prediction was
(so: measurement-H*Xhat=measurement-prediction).
K(called Kalman gain) is how much you want to correct your prediction(how much weight to give the measurement versus the prediction). K=P*H'/(H*P*H'+R), aka K is determined by taking the uncertainty of the system and combing it with the measurement noise covariance matrix(R).
for the covariance matrix(P):
P=[I-K*H]*P where I is the identity matrix. If Kalman gain is large then measurement is trusted meaning model was inaccurate so uncertainty needs to be changed. If Kalman gain is small then model is trusted meaning prediction mostly correct so uncertainty stays ruffly the same.
(note: you do correction first and then prediction because initial conditions are predictions)
In simple terms the KF takes input or model with uncertainty and compares it with other input with uncertainty to make a less uncertain output.
So a Kalman filter can be used with only one sensor because it can use its model to compare the sensor data to and so make the pos estimate more accurate than the sensors position data alone. But in practice it is often used with multiple sensors(sensor fusion).
Even though one sensor like for example a rotary encoder can be extremely accurate on its own, the Kalman filter makes the encoder more accurate when combined with a more noisy sensor like an IMU because the rotary encoder might miscount due to slipping and in this case the IMU could tell us that we have not moved.
Technically the Kalman filter can be used for any model and sensor input but it is most commonly found in robots with autonomous navigation since it is made for continuously changing systems and it is light on memory(only stores previous state).
MATLAB(open source version is Octave) is pretty much a powerful programmable calculator commonly used for simulation. Below are different Kalman filter implementations simulated using Octave.
Here is the most simple version of a Kalman filter. A 1D Kalman filter focusing only on position, estimated using IMU acceleration data in the X axis and GPS position data.
%1D example
%kalman filter IMU(accelerometer) x pos only + GPS x pos only
%kalman filter is a linear minimum variance algorithm/filter
%-----kalman filter basic idea------
%you predict with model your pos--> here we use IMU acc data as model
%then you compare your model with measured data to get your estimated pos
%--> we use GPS pos data as the measured data
%------------
%acc=accelerometer
clc; clear;
%------time------
%IMU data(acceleration) is measured more often then GPS data(pos)
dt = 0.01; % IMU timestep (100 Hz)
N = 1000;
t = (0:N-1) * dt;
gps_dt = 0.1; % GPS timestep (10 Hz)
gps_steps = round(gps_dt / dt); % GPS update interval
% -----------if no noise perfect data for comparision------------------
a_true = 0.5 * ones(1, N); % constant acceleration
%double integration (no filter) uncorrected data for comparison
%note: kalman filter has built in integration(takes a as input and x as output)
%initialized vx and xpos to zero
v_true = zeros(1, N);
x_true = zeros(1, N);
for k = 2:N
v_true(k) = v_true(k-1) + a_true(k) * dt;
x_true(k) = x_true(k-1) + v_true(k) * dt;
end
% -----------Accelerometer data with error------------
accel_bias = 0.1; %would get this from mean of acc data at rest
accel_noise_std = 0.2; %would get this from variance of acc data at rest minus bias
% Measured acceleration
axm= a_true + accel_bias + accel_noise_std * randn(1, N);
% -------GPS data with error---------------
%note: GPS bias is not ~const so cant get it from data at rest
%so gps bias is modeled as noise(aka not separete bias)
gps_noise_std = 0.5; %would get this from variance of GPS data at rest
z = NaN(1, N); % NaN = no GPS available
for k = 1:gps_steps:N
if rand > 0.1 % 10% chance GPS dropout
z(k) = x_true(k) + gps_noise_std * randn;
end
end
% -----------------------------
% Kalman Filter setup
% State = [position; velocity; accel_bias]
%bias is a state because kalman is a linear not an affine linear filter
%model:
% state: x_hat=A*x_hat + B*axm(k);
% output equation: y=H*x_hat + D*axm(k);
%but we assume D=0(aka input does not directly affect output)
%axm(k)=input and y=output=x_hat,
%Note Y not = to y, Y is just an intermediate step not output
% -----------------------------
x_hat = [0; 0; 0]; % initial state
P = diag([1, 1, 1]); % initial covariance
H = [1 0 0];
%assuming simple v=xdot and a=vdot model(no degrees of freedom restriction)
A = [1 dt -0.5*dt^2;
0 1 -dt;
0 0 1];
B = [0.5*dt^2;
dt;
0];
% Process noise(how wrong is my model/accelerometer noise)
G=B;%G=matrix for noise but often its same as B
Q=G*accel_noise_std^2*G';%using fwd euler for simplicity
% Measurement noise (how wrong is my pos measurment for update/correction)
R = gps_noise_std^2;
x_est = zeros(3, N);
% ----------- Kalman loop-----------------
for k = 1:N
% update/correction(only every GPS step)
%do not do z(k) = last_gps_value; aka hold GPS values cus introduces errors
if ~isnan(z(k))
Y = z(k) - H * x_hat;
Kyy = H * P * H' + R;
K = P * H' / Kyy;%kalman gain
x_hat = x_hat + K * Y;
P = (eye(3) - K * H) * P;
end
% Prediction(every IMU step)
x_hat = A * x_hat + B * axm(k);
P = A * P * A' + Q;
x_est(:, k) = x_hat;
end
% Plot
figure;
plot(t, x_true, 'k', t, x_est(1,:), 'b','LineWidth', 1.5);
legend('true', 'kalman estimate');
xlabel('time (s)');
ylabel('Displacement x (m)');
grid on;
title('Position Estimation with Accel Bias Kalman Filter');
This next one is also a 1D Kalman filter, but this time focusing on orientation only by combining IMU gyro data in 1 axis(rotation around z) and GPS heading data.
%kalman filter IMU(gyro) + GPS heading only
%kalman filter is a linear minimum variance algorithm/filter
%-----kalman filter basic idea------
%you predict your angle with model--> here we use IMU gyro(angle change) data as model
%then you compare your model with measured data to get your estimated pos
%--> we use GPS heading data as the measured data
%------------
%acc=accelerometer
clc; clear;
%anonymous funtion
wrapTo180 = @(ang) mod(ang + 180, 360) - 180;
wrapToPi = @(ang) mod(ang + pi, 2*pi) - pi;
%------time------
%IMU data(gyro) is measured more often then GPS data(heading)
dt = 0.01; % IMU timestep (100 Hz)
N = 1000;
t = (0:N-1) * dt;
gps_dt = 0.1; % GPS timestep (10 Hz)
gps_steps = round(gps_dt / dt); % GPS update interval
% -----------if no noise perfect data for comparision------------------
w_true = 0.5 * ones(1, N); % angular rate
%double integration (no filter) uncorrected data for comparison
%note: kalman filter has built in integration(takes w as input and theta as output)
%initialize theta to zero
theta_true = zeros(1, N);
for k = 2:N
theta_true(k) = theta_true(k-1) + w_true(k) * dt;
end
% -----------Gyro data with error------------
gyro_bias = 0.1; %would get this from mean of gyro data at rest
gyro_noise_std = 0.2; %would get this from variance of gyro data at rest minus bias
% Measured angle rate
wm= w_true + gyro_bias + gyro_noise_std * randn(1, N);
% -------GPS data with error---------------
%note: GPS bias is not ~const so cant get it from data at rest
%so gps bias is modeled as noise(aka not separete bias)
gps_noise_theta_std = 0.5; %would get this from variance of GPS heading data at constant speed and direction
z = NaN(1, N); % NaN = no GPS available
for k = 1:gps_steps:N
if rand > 0.1 % 10% chance GPS dropout
z(k) = theta_true(k) + gps_noise_theta_std * randn;
% wrap to [-pi, pi]
z(k) = wrapToPi(z(k));
end
end
% -----------------------------
% Kalman Filter setup
% State = [theta; gyro_bias]
%bias is a state because kalman is a linear not an affine linear filter
%model:
% state: x_hat=A*x_hat + B*wm(k);
% output equation: y=H*x_hat + D*wm(k);
%but we assume D=0(aka input does not directly affect output)
%wm(k)=input and y=output=x_hat=theta,
%Note Y not = to y, Y is just an intermediate step not output
% -----------------------------
x_hat = [0; 0]; % initial state
P = diag([1, 1]); % initial covariance
H = [1 0];
%assuming simple w=thetadot model(no degrees of freedom restriction)
A = [1 -dt;
0 1];
B = [dt;
0];
% Process noise(how wrong is my model/accelerometer noise)
G=B;%G=matrix for noise but often its same as B
Q=G*gyro_noise_std^2*G';%using fwd euler for simplicity
% Measurement noise (how wrong is my pos measurment for update/correction)
R = gps_noise_theta_std^2;
x_est = zeros(2, N);
% ----------- Kalman loop-----------------
for k = 1:N
% update/correction(only every GPS step)
%do not do z(k) = last_gps_value; aka hold GPS values cus introduces errors
if ~isnan(z(k))
Y = wrapToPi(z(k) - H*x_hat);
Kyy = H * P * H' + R;
K = P * H' / Kyy;%kalman gain
x_hat = x_hat + K * Y;
P = (eye(2) - K * H) * P;
end
% Prediction(every IMU step)
x_hat =A * x_hat + B * wm(k);
P = A * P * A' + Q;
x_est(:, k) = x_hat;
end
% Plot
figure;
plot(t, theta_true, 'k', t, x_est(1,:), 'b','LineWidth', 1.5);
legend('true', 'kalman estimate');
xlabel('time (s)');
ylabel('angle theta (rad)');
grid on;
title('angle/orientation Estimation with gyro Bias Kalman Filter');
Now moving on from the basic principle to something more realistic for a robot, this 2D Kalman filter uses the classic acceleration=derivative of velocity(a=vdot) and velocity=derivative of position(v=xdot). This model assumes X and Y coordinates are independent(in a car this is not the case because it can not move sideways, but for some robots or a car with mecanum wheels having x and y independent is realistic).
Note:In body frame this model uses a=vdot model which is linear and has x and y independent but because in the Kalman filter body frame measurements are converted to global frame heading is needed which technically correlates x and y and makes the Kalman filter nonlinear. However, x and y are still more independent then in a car.
In the previous models I assumed bias was constant but in reality IMU bias randomly drifts(random walk), so in an effort to make this Kalman filter more accurate I added random walk to the bias of the IMU. At least in this simulation it makes a big difference(try it, comment the current Qw and uncomment the Qw for constant bias right above and the Kalman estimate will be much noisier)
%2D example and combining gyro and acc using a=vdot model(vx and vy are independent)
%kalman filter
%this code has both gyro(theta) and acc(x,y) combined with gps pos(x,y) and heading
%kalman filter is a linear minimum variance algorithm/filter
%-----kalman filter basic idea------
%you predict with model your angle and pos
%--> here we use IMU gyro(angle change) and acc data as model
%then you compare your model with measured data to get your estimated pos
%--> we use GPS pos and heading data as the measured data
%------------
%acc=accelerometer
clc; clear;
%anonymous funtion
wrapTo180 = @(ang) mod(ang + 180, 360) - 180;
wrapToPi = @(ang) mod(ang + pi, 2*pi) - pi;
%------time------
%IMU data is measured more often then GPS data
dt = 0.01; % IMU timestep (100 Hz)
N = 1000;
t = (0:N-1) * dt;
gps_dt = 0.1; % GPS timestep (10 Hz)
gps_steps = round(gps_dt / dt); % GPS update interval
% -----------if no noise perfect data for comparision------------------
w_true = 0.5 * ones(1, N); % angular rate
ax_body_true = 0.5 * ones(1, N); % constant x acceleration(body frame)
ay_body_true = 0.5 * ones(1, N); % constant x acceleration(body frame)
%double integration (no filter) uncorrected data for comparison
%note: kalman filter has built in integration(takes w as input and theta as output)
%initialized to zero
theta_true = zeros(1, N);
vx_body_true = zeros(1, N);
x_true = zeros(1, N);
vy_body_true = zeros(1, N);
y_true = zeros(1, N);
for k = 2:N
theta_true(k) = theta_true(k-1) + w_true(k) * dt;
vx_body_true(k) = vx_body_true(k-1) + ax_body_true(k) * dt;
vy_body_true(k) = vy_body_true(k-1) + ay_body_true(k) * dt;
% Project velocities to global frame(body to earth rotation)
v_x = vx_body_true(k) * cos(theta_true(k)) - vy_body_true(k) * sin(theta_true(k));
v_y = vx_body_true(k) * sin(theta_true(k)) + vy_body_true(k) * cos(theta_true(k));
% Integrate to get global position
x_true(k) = x_true(k-1) + v_x * dt;
y_true(k) = y_true(k-1) + v_y * dt;
end
% -----------IMU data with error------------
gyro_bias = 0.1; %would get this from mean of gyro data at rest
gyro_noise_std = 0.2; %would get this from variance of gyro data at rest minus bias
accel_biasx = 0.1; %would get this from mean of acc data at rest
accel_noisex_std = 0.2; %would get this from variance of acc data at rest minus bias
accel_biasy = 0.1; %would get this from mean of acc data at rest
accel_noisey_std = 0.2; %would get this from variance of acc data at rest minus bias
%In theory bias is usually assumed consant for simplicity
%in practice it is better to assume bias drifts over time, allows for better accuracy
% Bias random walk std
%get this from datasheet and convert rw=noise density/(sqrt(time step))
accel_bias_rw_std = 0.01; % m/s^2 / sqrt(s)
gyro_bias_rw_std = 0.005; % rad/s / sqrt(s)
% Measured IMU data
wm= w_true + gyro_bias + gyro_noise_std * randn(1, N);
axm = ax_body_true + accel_biasx + accel_noisex_std * randn(1, N);
aym = ay_body_true + accel_biasy + accel_noisey_std * randn(1, N);
% -------GPS data with error---------------
%note: GPS bias is not ~const so cant get it from data at rest
%so gps bias is modeled as noise(aka not separete bias)
gps_noisex_std = 0.5; %would get this from variance of GPS data at rest
gps_noisey_std = 0.5; %would get this from variance of GPS data at rest
gps_noise_theta_std = 0.5; %would get this from variance of GPS data at constant speed and direction
z_x = NaN(1, N); % NaN = no GPS available
z_y = NaN(1, N); % NaN = no GPS available
z_theta = NaN(1, N); % NaN = no GPS available
for k = 1:gps_steps:N
if rand > 0.1 % 10% chance GPS dropout
z_theta(k) = theta_true(k) + gps_noise_theta_std * randn;
z_x(k) = x_true(k) + gps_noisex_std * randn;
z_y(k) = y_true(k) + gps_noisey_std * randn;
% wrap to [-pi, pi]
z_theta(k) = wrapToPi(z_theta(k));
end
end
% -----------------------------
% Kalman Filter setup
% State = [x; y; theta; vbodyx; vbodyy; accel_biasx; accel_biasy; gyro_bias]
%bias is a state because kalman is a linear not an affine linear filter
%model:
% state: x_hat=A*x_hat + B*input(k);
% output equation: output=H*x_hat + D*input;
%but we assume D=0(aka input does not directly affect output)
%[axm(k); aym(k); wm(k)]=input and output=x_hat=[x,y,theta]
% -----------------------------
x_hat = zeros(8,1); % initial state
x_pred = x_hat;
P = diag([1, 1, 1, 1, 1, 1, 1, 1]); % initial covariance
H = [1 0 0 0 0 0 0 0; % x
0 1 0 0 0 0 0 0; % y
0 0 1 0 0 0 0 0]; % theta
% Measurement noise (how wrong is my measurment for update/correction)
R = diag([gps_noisex_std^2, gps_noisey_std^2, gps_noise_theta_std^2]);
x_est = zeros(8, N);
% ----------- Kalman loop-----------------
%dont have A and B declaration because they change every step
%because need to linearize at current operating point every time
for k = 2:N
% update/correction(only every GPS step)
%do not do z(k) = last_gps_value; aka hold GPS values cus introduces errors
z = [z_x(k); z_y(k); z_theta(k)];
if all(~isnan(z))
Y = z - H*x_pred;
Y(3) = wrapToPi(Y(3));
Kyy = H * P * H' + R;
K = P * H' / Kyy;%kalman gain
x_hat = x_pred + K * Y;
I = eye(8);
P = (I - K*H) * P * (I - K*H)' + K*R*K';
else
%in 1D we did not have this line because used linear model(now its nonlinear)
%so x_hat was updated with prediction directly not over x_pred
x_hat = x_pred;%if no measurment then best estimate is prediction
end
% --------Prediction(every IMU step)--------
theta = x_hat(3);
vx = x_hat(4); vy = x_hat(5);
bax = x_hat(6); bay = x_hat(7); bw = x_hat(8);
% Nonlinear state propagation
x_pred = zeros(8,1);
x_pred(1) = x_hat(1) + dt*(vx*cos(theta) - vy*sin(theta));
x_pred(2) = x_hat(2) + dt*(vx*sin(theta) + vy*cos(theta));
x_pred(3) = theta + dt*(wm(k) - bw);
x_pred(4) = vx + dt*(axm(k) - bax);
x_pred(5) = vy + dt*(aym(k) - bay);
x_pred(6) = bax;
x_pred(7) = bay;
x_pred(8) = bw;
% Jacobian A
A = eye(8);
A(1,3) = dt*(-vx*sin(theta) - vy*cos(theta));
A(1,4) = dt*cos(theta);
A(1,5) = -dt*sin(theta);
A(2,3) = dt*(vx*cos(theta) - vy*sin(theta));
A(2,4) = dt*sin(theta);
A(2,5) = dt*cos(theta);
A(3,8) = -dt; % theta affected by gyro bias
A(4,6) = -dt; % vx affected by accel_biasx
A(5,7) = -dt; % vy affected by accel_biasy
G = zeros(8,6);
%the next 4 lines are commented out because
%ax and ay noise already injected into velocity(see below)
%so injecting into position aswell is unnecessary
% G(1,1) = dt * cos(theta); % x affected by ax_noise
% G(1,2) = -dt * sin(theta); % x affected by ay_noise
% G(2,1) = dt * sin(theta); % y affected by ax_noise
% G(2,2) = dt * cos(theta); % y affected by ay_noise
G(3,3) = dt;
G(4,1) = dt; % vx affected by ax_noise
G(5,2) = dt; % vy affected by ay_noise
G(6,3) = dt; % bax random walk
G(7,4) = dt; % bay random walk
G(8,5) = dt; % gyro bias random walk
% Process noise(how wrong is my model/accelerometer+gyro noise)
%Qw = diag([accel_noisex_std^2, accel_noisey_std^2, gyro_noise_std^2, accel_biasx^2, accel_biasy^2, gyro_bias^2]); %if bias was constant use this
Qw = diag([accel_noisex_std^2, accel_noisey_std^2, gyro_noise_std^2, accel_bias_rw_std^2*dt, accel_bias_rw_std^2*dt, gyro_bias_rw_std^2*dt]); %bias with random walk
Q=G*Qw*G';
% Covariance prediction
P = A*P*A' + Q;
x_est(:, k) = x_pred;
end
x_est_out = [x_est(1,:); % x pos
x_est(2,:); % y pos
x_est(3,:)]; % heading theta
% Plot
% X position
subplot(3,1,1);
plot(t, x_true, 'k', t, x_est_out(1,:), 'b','LineWidth',1.5);
xlabel('Time (s)'); ylabel('X (m)');
legend('true', 'kalman estimate');
grid on;
title('X Position');
title('pos and orientation/angle Estimation with gyro and accelerometer Bias Kalman Filter');
% Y position
subplot(3,1,2);
plot(t, y_true, 'k', t, x_est_out(2,:), 'b','LineWidth',1.5);
xlabel('Time (s)'); ylabel('Y (m)');
legend('true', 'kalman estimate');
grid on;
title('Y Position');
% Heading theta
subplot(3,1,3);
plot(t, theta_true, 'k', t, x_est_out(3,:), 'b','LineWidth',1.5);
xlabel('Time (s)'); ylabel('Theta (rad)');
legend('true', 'kalman estimate');
grid on;
title('Heading \theta');
Now modifying the model to have x and y dependent like in a car yields this 2D Kalman filter.
%2D example and combining gyro and acc using steering restricted model(nonholonomic) instead of basic a=vdot model
%here vy depends on vx
%model used:
%xdot=v*cos(theta)
%ydot=v*sin(theta)
%thetadot=w
%kalman filter
%this code has both gyro(theta) and acc(x,y) combined with gps pos(x,y) and heading
%kalman filter is a linear minimum variance algorithm/filter
%-----kalman filter basic idea------
%you predict with model your angle and pos
%--> here we use IMU gyro(angle change) and acc data as model
%then you compare your model with measured data to get your estimated pos
%--> we use GPS pos and heading data as the measured data
%------------
%acc=accelerometer
clc; clear;
%anonymous funtion
wrapTo180 = @(ang) mod(ang + 180, 360) - 180;
wrapToPi = @(ang) mod(ang + pi, 2*pi) - pi;
%------time------
%IMU data is measured more often then GPS data
dt = 0.01; % IMU timestep (100 Hz)
N = 1000;
t = (0:N-1) * dt;
gps_dt = 0.1; % GPS timestep (10 Hz)
gps_steps = round(gps_dt / dt); % GPS update interval
% -----------if no noise perfect data for comparision------------------
w_true = 0.5 * ones(1, N); % angular rate
ax_body_true = 0.5 * ones(1, N); % constant x acceleration(body frame)
%no ay_body_true because y motion is coupled to x
%double integration (no filter) uncorrected data for comparison
%note: kalman filter has built in integration(takes w as input and theta as output)
%initialized to zero
theta_true = zeros(1, N);
vx_body_true = zeros(1, N);
x_true = zeros(1, N);
y_true = zeros(1, N);
for k = 2:N
theta_true(k) = theta_true(k-1) + w_true(k) * dt;
vx_body_true(k) = vx_body_true(k-1) + ax_body_true(k) * dt;
% Nonholonomic kinematics
x_true(k) = x_true(k-1) + dt * vx_body_true(k) * cos(theta_true(k));
y_true(k) = y_true(k-1) + dt * vx_body_true(k) * sin(theta_true(k));
end
% -----------IMU data with error------------
gyro_bias = 0.1; %would get this from mean of gyro data at rest
gyro_noise_std = 0.2; %would get this from variance of gyro data at rest minus bias
accel_biasx = 0.1; %would get this from mean of acc data at rest
accel_noisex_std = 0.2; %would get this from variance of acc data at rest minus bias
%In theory bias is usually assumed consant for simplicity
%in practice it is better to assume bias drifts over time, allows for better accuracy
% Bias random walk std
%get this from datasheet and convert rw=noise density/(sqrt(time step))
accel_bias_rw_std = 0.01; % m/s^2 / sqrt(s)
gyro_bias_rw_std = 0.005; % rad/s / sqrt(s)
% Measured IMU data
wm= w_true + gyro_bias + gyro_noise_std * randn(1, N);
axm = ax_body_true + accel_biasx + accel_noisex_std * randn(1, N);
% -------GPS data with error---------------
%note: GPS bias is not ~const so cant get it from data at rest
%so gps bias is modeled as noise(aka not separete bias)
gps_noisex_std = 0.5; %would get this from variance of GPS data at rest
gps_noisey_std = 0.5; %would get this from variance of GPS data at rest
gps_noise_theta_std = 0.5; %would get this from variance of GPS data at constant speed and direction
z_x = NaN(1, N); % NaN = no GPS available
z_y = NaN(1, N); % NaN = no GPS available
z_theta = NaN(1, N); % NaN = no GPS available
for k = 1:gps_steps:N
if rand > 0.1 % 10% chance GPS dropout
z_theta(k) = theta_true(k) + gps_noise_theta_std * randn;
z_x(k) = x_true(k) + gps_noisex_std * randn;
z_y(k) = y_true(k) + gps_noisey_std * randn;
% wrap to [-pi, pi]
z_theta(k) = wrapToPi(z_theta(k));
end
end
% -----------------------------
% Kalman Filter setup
% State = [x; y; theta; vx; accel_biasx; gyro_bias]
%bias is a state because kalman is a linear not an affine linear filter
%model:
% state: x_hat=A*x_hat + B*input(k);
% output equation: output=H*x_hat + D*input;
%but we assume D=0(aka input does not directly affect output)
%[axm(k); wm(k)]=input and output=x_hat=[x,y,theta]
% -----------------------------
x_hat = zeros(6,1); % initial state
x_pred = x_hat;
P = diag([1, 1, 1, 1, 1, 1]); % initial covariance
H = [1 0 0 0 0 0; % x
0 1 0 0 0 0; % y
0 0 1 0 0 0]; % theta
% Measurement noise (how wrong is my measurment for update/correction)
R = diag([gps_noisex_std^2, gps_noisey_std^2, gps_noise_theta_std^2]);
x_est = zeros(6, N);
% ----------- Kalman loop-----------------
%dont have A and B declaration because they change every step
%because need to linearize at current operating point every time
for k = 2:N
% -------update/correction(only every GPS step)-------
%do not do z(k) = last_gps_value; aka hold GPS values cus introduces errors
z = [z_x(k); z_y(k); z_theta(k)];
if all(~isnan(z))
Y = z - H*x_pred;
Y(3) = wrapToPi(Y(3));
Kyy = H * P * H' + R;
K = P * H' / Kyy;%kalman gain
x_hat = x_pred + K * Y;
%P = (eye(6) - K * H) * P;
I = eye(6);
P = (I - K*H) * P * (I - K*H)' + K*R*K';
else
%in 1D we did not have this line because used linear model(now its nonlinear)
%so x_hat was updated with prediction directly not over x_pred
x_hat = x_pred;%if no measurment then best estimate is prediction
end
% --------Prediction(every IMU step)--------
theta = x_hat(3);
vx = x_hat(4);
bax = x_hat(5);
bw = x_hat(6);
% Nonlinear state propagation
x_pred = zeros(6,1);
x_pred(1) = x_hat(1) + dt*(vx*cos(theta));
x_pred(2) = x_hat(2) + dt*(vx*sin(theta));
x_pred(3) = theta + dt*(wm(k) - bw);
x_pred(4) = vx + dt*(axm(k) - bax);
x_pred(5) = bax;
x_pred(6) = bw;
% Jacobian A
A = eye(6);
A(1,3) = -dt*(vx*sin(theta));
A(1,4) = dt*cos(theta);
A(2,3) = dt*(vx*cos(theta));
A(2,4) = dt*sin(theta);
A(3,6) = -dt;
A(4,5) = -dt;
G = zeros(6,4);
%the next 2 lines are commented out because
%ax and ay noise already injected into velocity(see below)
%so injecting into position aswell is unnecessary
%G(1,1) = dt * cos(theta); % x affected by accel noise
%G(2,1) = dt * sin(theta); % y affected by accel noise
G(3,2) = dt; % theta affected by gyro noise
G(4,1) = dt; % velocity affected by accel noise
G(5,3) = 1; % accel bias random walk
G(6,4) = 1; % gyro bias random walk
% Process noise(how wrong is my model/accelerometer+gyro noise)
%Qw=diag([accel_noisex_std^2, gyro_noise_std^2, accel_biasx^2, gyro_bias^2]);%%if bias was constant use this
Qw = diag([accel_noisex_std^2, gyro_noise_std^2, accel_bias_rw_std^2*dt, gyro_bias_rw_std^2*dt]); %bias with random walk
Q = G * Qw * G';
% Covariance prediction
P = A*P*A' + Q;
x_est(:, k) = x_pred;
end
x_est_out = [x_est(1,:); % x pos
x_est(2,:); % y pos
x_est(3,:)]; % heading theta
% Plot
% X position
subplot(3,1,1);
plot(t, x_true, 'k', t, x_est_out(1,:), 'b','LineWidth',1.5);
xlabel('Time (s)'); ylabel('X (m)');
legend('true', 'kalman estimate');
grid on;
title('X Position');
title('pos and orientation/angle Estimation with gyro and accelerometer Bias Kalman Filter');
% Y position
subplot(3,1,2);
plot(t, y_true, 'k', t, x_est_out(2,:), 'b','LineWidth',1.5);
xlabel('Time (s)'); ylabel('Y (m)');
legend('true', 'kalman estimate');
grid on;
title('Y Position');
% Heading theta
subplot(3,1,3);
plot(t, theta_true, 'k', t, x_est_out(3,:), 'b','LineWidth',1.5);
xlabel('Time (s)'); ylabel('Theta (rad)');
legend('true', 'kalman estimate');
grid on;
title('Heading \theta');
Adding 2 encoders(one on the left and the other on the right caterpillar track motor) for more accuracy. The previous one worked for both tracked vehicles and cars but this one can only be used for vehicles that use differential steering because otherwise encoders can not detect heading(in a car the right and the left wheel turn almost the same amount so encoders cannot accuratly detect a turn)
%2D example and combining gyro, acc, GPS and wheel encoders
%steering restricted model (nonholonomic)
%only works for vehicles with differential steering due to encoders
%if use encoders only for position not heading then dont have differential steering only restriction
%model used:
%xdot=v*cos(theta)
%ydot=v*sin(theta)
%thetadot=w
clc; clear;
wrapTo180 = @(ang) mod(ang + 180, 360) - 180;
wrapToPi = @(ang) mod(ang + pi, 2*pi) - pi;
%------time------
dt = 0.01; % IMU timestep (100 Hz)
N = 1000;
t = (0:N-1) * dt;
gps_dt = 0.1; % GPS timestep (10 Hz)
gps_steps = round(gps_dt / dt); % GPS update interval
% -----------if no noise perfect data for comparision------------------
w_true = 0.5 * ones(1, N); % angular rate
ax_body_true = 0.5 * ones(1, N); % constant x acceleration(body frame)
%no ay_body_true because y motion is coupled to x
%double integration (no filter) uncorrected data for comparison
%note: kalman filter has built in integration(takes w as input and theta as output)
%initialized to zero
theta_true = zeros(1, N);
vx_body_true = zeros(1, N);
x_true = zeros(1, N);
y_true = zeros(1, N);
for k = 2:N
theta_true(k) = theta_true(k-1) + w_true(k) * dt;
vx_body_true(k) = vx_body_true(k-1) + ax_body_true(k) * dt;
% Nonholonomic kinematics
x_true(k) = x_true(k-1) + dt * vx_body_true(k) * cos(theta_true(k));
y_true(k) = y_true(k-1) + dt * vx_body_true(k) * sin(theta_true(k));
end
% -----------IMU data with error------------
gyro_bias = 0.1; %would get this from mean of gyro data at rest
gyro_noise_std = 0.2; %would get this from variance of gyro data at rest minus bias
accel_biasx = 0.1; %would get this from mean of acc data at rest
accel_noisex_std = 0.2; %would get this from variance of acc data at rest minus bias
%In theory bias is usually assumed consant for simplicity
%in practice it is better to assume bias drifts over time, allows for better accuracy
% Bias random walk std
%get this from datasheet and convert rw=noise density/(sqrt(time step))
accel_bias_rw_std = 0.01; % m/s^2 / sqrt(s)
gyro_bias_rw_std = 0.005; % rad/s / sqrt(s)
% Measured IMU data
wm = w_true + gyro_bias + gyro_noise_std * randn(1, N);
axm = ax_body_true + accel_biasx + accel_noisex_std * randn(1, N);
% -----------Encoder data with error------------------
wheel_base = 0.5; % meters
enc_noise_std = 0.05; % m/s
vL_true = vx_body_true - w_true * wheel_base/2;
vR_true = vx_body_true + w_true * wheel_base/2;
vL_m = vL_true + enc_noise_std * randn(1, N);
vR_m = vR_true + enc_noise_std * randn(1, N);
% -----------GPS data with error-----------------------
%note: GPS bias is not ~const so cant get it from data at rest
%so gps bias is modeled as noise(aka not separete bias)
gps_noisex_std = 0.5; %would get this from variance of GPS data at rest
gps_noisey_std = 0.5; %would get this from variance of GPS data at rest
gps_noise_theta_std = 0.5; %would get this from variance of GPS data at constant speed and direction
z_x = NaN(1, N); % NaN = no GPS available
z_y = NaN(1, N); % NaN = no GPS available
z_theta = NaN(1, N); % NaN = no GPS available
for k = 1:gps_steps:N
if rand > 0.1 % 10% chance GPS dropout
z_x(k) = x_true(k) + gps_noisex_std * randn;
z_y(k) = y_true(k) + gps_noisey_std * randn;
z_theta(k) = wrapToPi(theta_true(k) + gps_noise_theta_std * randn);
end
end
% -----------------------------
% Kalman Filter setup
% State = [x; y; theta; vx; accel_biasx; gyro_bias]
% -----------------------------
x_hat = zeros(6,1); % initial state
x_pred = x_hat;
P = diag([1 1 1 1 1 1]); % initial covariance
H_gps = [1 0 0 0 0 0; % x
0 1 0 0 0 0; % y
0 0 1 0 0 0]; % theta
H_enc = [0 0 0 1 0 wheel_base/2;
0 0 0 1 0 -wheel_base/2];
% Measurement noise (how wrong is my measurment for update/correction)
R_gps = diag([gps_noisex_std^2, gps_noisey_std^2, gps_noise_theta_std^2]);
x_est = zeros(6, N);
% ----------- Kalman loop-----------------
for k = 2:N
% --------Update/correction(GPS + Encoders)--------
Hk = [];
residual_k = [];
Rk = [];
% GPS
z_gps = [z_x(k); z_y(k); z_theta(k)];
y_gps = z_gps - H_gps * x_pred;
if all(~isnan(z_gps))
Hk = [Hk; H_gps];
residual_k = [residual_k; y_gps];
Rk = blkdiag(Rk, R_gps);
end
% Encoders (always available)
z_enc = [vL_m(k); vR_m(k)];
h_enc = [ x_pred(4) - (wm(k) - x_pred(6)) * wheel_base/2;
x_pred(4) + (wm(k) - x_pred(6)) * wheel_base/2 ];
R_enc = diag([enc_noise_std^2 enc_noise_std^2]);
y_enc = z_enc - h_enc;
Hk = [Hk; H_enc];
residual_k = [residual_k; y_enc];
Rk = blkdiag(Rk, R_enc);
% Apply correction
%y = residual_k - Hk * x_pred;
y = residual_k;
% Wrap only the theta innovation
theta_idx = find(sum(Hk(:,3),2) == 1); % rows measuring theta
for i = theta_idx'
y(i) = wrapToPi(y(i));
end
K = P * Hk' / (Hk * P * Hk' + Rk);
x_hat = x_pred + K * y;
I = eye(6);
P = (I - K*Hk)*P*(I - K*Hk)' + K*Rk*K';
% --------Prediction(every IMU step)--------
theta = x_hat(3);
vx = x_hat(4);
bax = x_hat(5);
bw = x_hat(6);
% Nonlinear state propagation
x_pred = zeros(6,1);
x_pred(1) = x_hat(1) + dt * vx * cos(theta);
x_pred(2) = x_hat(2) + dt * vx * sin(theta);
x_pred(3) = theta + dt * (wm(k) - bw);
x_pred(4) = vx + dt * (axm(k) - bax);
x_pred(5) = bax;
x_pred(6) = bw;
% Jacobian A
A = eye(6);
A(1,3) = -dt * vx * sin(theta);
A(1,4) = dt * cos(theta);
A(2,3) = dt * vx * cos(theta);
A(2,4) = dt * sin(theta);
A(3,6) = -dt;
A(4,5) = -dt;
G = zeros(6,4);
%the next 2 lines are commented out because
%ax and ay noise already injected into velocity(see below)
%so injecting into position aswell is unnecessary
%G(1,1) = dt * cos(theta); % x affected by accel noise
%G(2,1) = dt * sin(theta); % y affected by accel noise
G(3,2) = dt; % theta affected by gyro noise
G(4,1) = dt; % velocity affected by accel noise
G(5,3) = 1; % accel bias random walk
G(6,4) = 1; % gyro bias random walk
% Process noise(how wrong is my model/accelerometer+gyro noise)
%Qw=diag([accel_noisex_std^2, gyro_noise_std^2, accel_biasx^2, gyro_bias^2]);%if bias was constant use this
Qw = diag([accel_noisex_std^2, gyro_noise_std^2, accel_bias_rw_std^2*dt, gyro_bias_rw_std^2*dt]); %bias with random walk
Q = G * Qw * G';
% Covariance prediction
P = A*P*A' + Q;
x_est(:,k) = x_pred;
end
% -----------Plots-----------------
figure;
subplot(3,1,1);
plot(t,x_true,'k',t,x_est(1,:),'b','LineWidth',1.5);
legend('true', 'kalman estimate');
grid on;
ylabel('X (m)');
title('pos and orientation/angle Estimation with gyro and accelerometer, GPS, encoder Kalman Filter');
subplot(3,1,2);
plot(t,y_true,'k',t,x_est(2,:),'b','LineWidth',1.5);
legend('true', 'kalman estimate');
grid on;
ylabel('Y (m)');
subplot(3,1,3);
plot(t,theta_true,'k',t,x_est(3,:),'b','LineWidth',1.5);
legend('true', 'kalman estimate');
grid on;
xlabel('Time (s)');
ylabel('\theta (rad)');
Enough theory, if it does not work in practice the theory does not matter. Its time for real KFC(Kalman filter code, no not Kentucky fried chicken :) ). Here is a 1D implementation using a rotary encoder and IMU(MPU-6050).
Rotary encoder with polling inside a timer interrupt is the best way to software debounce the encoder because the polling makes sure it does not miscount due to noise. Doing it via interrupt would allow noise to trigger it because the transition phase might not be sharp on off so it might trigger a second time by accident. And the timer interrupt makes sure the polling happens at regular intervals no matter what other things are happening in the program.
I know in the octave codes I wrote how to measure bias and noise standard deviations but honestly just playing with the numbers a bit is good enough.
By the way Arduino is technically not the best option for this since KFs can get computationally expensive if the matrices get very big so if I was not just testing this out but actually building a full robot something like raspberry pi would probably be better.
/***********
*IMU: vcc=3.3v,GND,scl=A5,sda=A4, AD0=GND
*encoder:vcc=5v, GND, clk DT
* https://lastminuteengineers.com/rotary-encoder-arduino-tutorial/
* encoders, use polling if dont want to do hardware debounce
*and timer interrupt to guarante the polling is regular
*doing interrupt for encoders(no polling) allows noise to misfire interrupts
*since not doing interrupts,
*dont have to use pins 2 and 3(the interrupt pins)
*can use any(i believe) other pin
*IMU with corrections(mohany filter, remove gravity, remove bias,
*temperature drift compensation, linear acceleration filter)
**********/
#include <Wire.h>
//===IMU=====
#define MPU_ADDR 0x68
#define ACCEL_SCALE (1.0 / 16384.0) //±2g
#define GYRO_SCALE (250.0 / 32768.0) //±250 dps
// ==rotary encoder=======
//#define SW 6(switch not needed here)
#define CLK1 2
#define DT1 4
//===IMU variables====
int16_t ax, ay, az;//acceleration
int16_t gx, gy, gz;//angular velocity
int16_t Temp_raw;
float Temp_C;
float refTemp = 0;
float tempCoeff[3] = {0.05, 0.05, 0.05}; //deg/s per °C
float gyroBias[3] = {0, 0, 0};
bool gyroCalibrated = false;
float linAccFilt[3] = {0, 0, 0};
float alpha = 0.1; //accel low-pass filter
//Quaternion
float q[4] = {1, 0, 0, 0};
//Mahony filter parameters
float Kp = 20.0;
float Ki = 0.0;
float ix = 0, iy = 0, iz = 0;
//===rotary encoder variables
//(measures relative pos(not absolute like gps),
//so for error not to grow we do pos/time(aka velocity))====
volatile int cnt1 = 0;
volatile int lastcnt1 = 0;
volatile bool lastCLK1;
unsigned long lastenctime = 0;
#define encinterval 100 //Calculate speed every 100ms
#define WHEELD 0.065 //wheel diameter
#define CIRCUMFERENCE PI*WHEELD
#define CNTPERREV 20
#define MPERCNT CIRCUMFERENCE/CNTPERREV //meters per count
//===Kalman filter variables====
//State
float x_pos = 0;
float x_vel = 0;
float x_bias = 0;
//Covariance
float P[3][3] = {
{1, 0, 0},
{0, 1, 0},
{0, 0, 1}
};
//Noise (TUNE THESE)
float accel_noise_std=0.5;//m/s^2
float encoder_noise_std=0.05;//meters/s
float bias_noise_std=0.01;//m/s^2 / sqrt(s)
//=======
unsigned long lastMicros;
//===========
void setup(){
Serial.begin(9600);
Wire.begin();
//IMU init
Wire.beginTransmission(MPU_ADDR);
Wire.write(0x6B);
Wire.write(0);
Wire.endTransmission(true);
calibrateGyro();
//Encoder
pinMode(CLK1, INPUT_PULLUP);
pinMode(DT1, INPUT_PULLUP);
lastCLK1 = digitalRead(CLK1);
//=======TIMER1 SETUP (Polling every 1ms)=======
noInterrupts();
TCCR1A = 0;
TCCR1B = 0;
TCNT1 = 0;//initialize counter value to 0
//set compare match register for 1000Hz increments
//formula:(16MHz / (prescaler * desired_freq)) - 1
OCR1A = 249;// = (16,000,000 / (64 * 1000)) - 1
TCCR1B |= (1 << WGM12);//turn on CTC mode
TCCR1B |= (1 << CS11) | (1 << CS10); //set CS11 and CS10 bits for 64 prescaler
TIMSK1 |= (1 << OCIE1A);//enable timer compare interrupt
interrupts();//enable all interrupts
lastMicros = micros();
Serial.println("1D Kalman Filter: IMU + Encoder");
}
//=============
void loop(){
//=====Read IMU=======
Wire.beginTransmission(MPU_ADDR);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(MPU_ADDR, 14, true);
ax = Wire.read() << 8 | Wire.read();
ay = Wire.read() << 8 | Wire.read();
az = Wire.read() << 8 | Wire.read();
Temp_raw = Wire.read() << 8 | Wire.read();
gx = Wire.read() << 8 | Wire.read();
gy = Wire.read() << 8 | Wire.read();
gz = Wire.read() << 8 | Wire.read();
//=======Time step========
unsigned long tnow = micros();
float dt = (tnow - lastMicros) * 1e-6;
lastMicros = tnow;
//======Accel in g========
float ax_g = ax * ACCEL_SCALE;
float ay_g = ay * ACCEL_SCALE;
float az_g = az * ACCEL_SCALE;
//========Gyro rad/s=======
float gx_r = (gx - gyroBias[0]) * GYRO_SCALE * DEG_TO_RAD;
float gy_r = (gy - gyroBias[1]) * GYRO_SCALE * DEG_TO_RAD;
float gz_r = (gz - gyroBias[2]) * GYRO_SCALE * DEG_TO_RAD;
//=====temperature=====
Temp_C = (Temp_raw / 340.0) + 36.53;
float tempError = Temp_C - refTemp;
gx_r -= tempCoeff[0] * tempError * DEG_TO_RAD;
gy_r -= tempCoeff[1] * tempError * DEG_TO_RAD;
gz_r -= tempCoeff[2] * tempError * DEG_TO_RAD;
//=======Mahony========
MahonyUpdate(ax_g, ay_g, az_g, gx_r, gy_r, gz_r, dt);
//========Gravity========
float grav[3];
grav[0] = 2 * (q[1]*q[3] - q[0]*q[2]);
grav[1] = 2 * (q[0]*q[1] + q[2]*q[3]);
grav[2] = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];
//=======Linear acceleration========
float linAcc[3];
linAcc[0] = ax_g - grav[0];
linAcc[1] = ay_g - grav[1];
linAcc[2] = az_g - grav[2];
//linear acc filter
for(int i = 0; i < 3; i++){
linAccFilt[i] = alpha * linAcc[i] + (1 - alpha) * linAccFilt[i];
}
float ax_mps2 = linAccFilt[0] * 9.80665;
//=======Kalman predict(IMU)========
kalmanPredict(ax_mps2, dt);
//======Kalman update/correct(encoder)=======
static unsigned long lastEncUpdate = 0;
if(millis() - lastEncUpdate > 100){//10Hz
lastEncUpdate = millis();
int c1=cnt1;
unsigned long now = millis();
if(now - lastenctime >= encinterval){//dont read speed to often
float enc1Vel=(float)((c1-lastcnt1)*MPERCNT)/((now-lastenctime)* 1e-3);
lastcnt1=c1;
//======Print=========
Serial.print("enc1Vel: ");
Serial.print(enc1Vel);
//Serial.print((float)encoderCount, 3);
Serial.print(" | KF POS: ");
Serial.print(x_pos, 3);
Serial.print(" | KF VEL: ");
Serial.println(x_vel, 3);
kalmanUpdateVelocity(enc1Vel);
lastenctime=now;
}
}
}
//===encoder ISR===
//timer interrupt to do polling of encoders
ISR(TIMER1_COMPA_vect){
//ENCODER 1
bool currentCLK1 = digitalRead(CLK1);
if(currentCLK1 != lastCLK1){
if(currentCLK1 == LOW){
if(digitalRead(DT1) != currentCLK1){
cnt1++;
} else{
cnt1--;
}
}
}
lastCLK1 = currentCLK1;
}
//====Kalman fcts=====
void kalmanPredict(float acc, float dt){
float dt2 = dt * dt;
// State prediction
x_pos += x_vel * dt + 0.5f * (acc - x_bias) * dt2;
x_vel += (acc - x_bias) * dt;
float A[3][3] = {
{1, dt, -0.5f * dt2},
{0, 1, -dt},
{0, 0, 1}
};
float AT[3][3] = {
{1, 0, 0},
{dt, 1, 0},
{-0.5f*dt2, -dt, 1}
};
//P = A*P*A'
float Pn[3][3] = {0};
for(int i=0;i<3;i++){
for(int j=0;j<3;j++){
for(int k=0;k<3;k++){
Pn[i][j] += A[i][k] * P[k][j];
}
}
}
for(int i=0;i<3;i++){
for(int j=0;j<3;j++){
P[i][j] = 0;
for(int k=0;k<3;k++){
P[i][j] += Pn[i][k] * AT[k][j];
}
}
}
//Process noise Q = G*?²*G'
float G[3] = {0.5f * dt2, dt, 0.0f};
float var = accel_noise_std * accel_noise_std;
for(int i=0;i<3;i++){
for(int j=0;j<3;j++){
P[i][j] += var * G[i] * G[j];
}
}
//bias noise
P[2][2] += bias_noise_std * bias_noise_std * dt;
}
/*void kalmanUpdate(float z){
float y = z - x_pos;
float S = P[0][0] + encoder_noise_std * encoder_noise_std;
float K[3];
K[0] = P[0][0] / S;
K[1] = P[1][0] / S;
K[2] = P[2][0] / S;
x_pos += K[0] * y;
x_vel += K[1] * y;
x_bias += K[2] * y;
float P_new[3][3];
for(int i=0;i<3;i++){
for(int j=0;j<3;j++){
P_new[i][j] = P[i][j] - K[i] * P[0][j];
}
}
memcpy(P, P_new, sizeof(P));
}*/
void kalmanUpdateVelocity(float z){
//Measurement: z = velocity
float y = z - x_vel;
float R = encoder_noise_std * encoder_noise_std;
float S = P[1][1] + R;
float K[3];
K[0] = P[0][1] / S;
K[1] = P[1][1] / S;
K[2] = P[2][1] / S;
//State update
x_pos += K[0] * y;
x_vel += K[1] * y;
x_bias += K[2] * y;
//Covariance update: P = (I - K H) P
//H = [0 1 0]
float P_new[3][3];
for(int i = 0; i < 3; i++){
for(int j = 0; j < 3; j++){
P_new[i][j] = P[i][j] - K[i] * P[1][j];
}
}
memcpy(P, P_new, sizeof(P));//copy memory block(dest, source, size)
}
//=======calibration=======
void calibrateGyro(){
const int samples = 500;
long sum[3] = {0, 0, 0};
int16_t tempRaw=0;
Serial.println("Calibrating gyro... keep IMU still");
for(int i = 0; i < samples; i++){
Wire.beginTransmission(MPU_ADDR);
Wire.write(0x41); //start at TEMP_OUT_H
Wire.endTransmission(false);
Wire.requestFrom(MPU_ADDR, 8, true);
tempRaw = Wire.read() << 8 | Wire.read();
int16_t gx = Wire.read() << 8 | Wire.read();
int16_t gy = Wire.read() << 8 | Wire.read();
int16_t gz = Wire.read() << 8 | Wire.read();
sum[0] += gx;
sum[1] += gy;
sum[2] += gz;
delay(5);
}
gyroBias[0] = sum[0] / (float)samples;
gyroBias[1] = sum[1] / (float)samples;
gyroBias[2] = sum[2] / (float)samples;
gyroCalibrated = true;
//get reference temperature
refTemp = (tempRaw / 340.0) + 36.53;
Serial.print("temp:");
Serial.println(refTemp);
Serial.println("Gyro and temp calibration done");
}
//===Mahony filter====
void MahonyUpdate(float ax, float ay, float az, float gx, float gy, float gz, float dt){
float recipNorm;
float vx, vy, vz;
float ex, ey, ez;
float q0=q[0], q1=q[1], q2=q[2], q3=q[3];
float norm = ax*ax + ay*ay + az*az;
if(norm > 0){
recipNorm = 1.0 / sqrt(norm);
ax *= recipNorm; ay *= recipNorm; az *= recipNorm;
vx = 2*(q1*q3 - q0*q2);
vy = 2*(q0*q1 + q2*q3);
vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
ex = (ay*vz - az*vy);
ey = (az*vx - ax*vz);
ez = (ax*vy - ay*vx);
gx += Kp * ex;
gy += Kp * ey;
gz += Kp * ez;
}
gx *= 0.5 * dt;
gy *= 0.5 * dt;
gz *= 0.5 * dt;
q[0] += (-q1*gx - q2*gy - q3*gz);
q[1] += ( q0*gx + q2*gz - q3*gy);
q[2] += ( q0*gy - q1*gz + q3*gx);
q[3] += ( q0*gz + q1*gy - q2*gx);
recipNorm = 1.0 / sqrt(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
q[0]*=recipNorm; q[1]*=recipNorm;
q[2]*=recipNorm; q[3]*=recipNorm;
}
Last but not least lets add a second encoder so that we can measure steering angle.
/***********
*IMU: vcc=3.3v,GND,scl=A5,sda=A4, AD0=GND
*encoder:vcc=5v, GND, clk DT
* https://lastminuteengineers.com/rotary-encoder-arduino-tutorial/
* encoders, use polling if dont want to do hardware debounce
*and timer interrupt to guarante the polling is regular
*doing interrupt for encoders(no polling) allows noise to misfire interrupts
*since not doing interrupts,
*dont have to use pins 2 and 3(the interrupt pins)
*can use any(i believe) other pin
*IMU with corrections(mohany filter, remove gravity, remove bias,
*temperature drift compensation, linear acceleration filter)
**********/
#include <Wire.h>
//===IMU=====
#define MPU_ADDR 0x68
#define ACCEL_SCALE (1.0 / 16384.0) //±2g
#define GYRO_SCALE (250.0 / 32768.0) //±250 dps
// ==rotary encoder=======
//#define SW 6(switch not needed here)
#define CLK1 2
#define DT1 4
#define CLK2 3
#define DT2 5
//===IMU variables====
int16_t ax, ay, az;//acceleration
int16_t gx, gy, gz;//angular velocity
int16_t Temp_raw;
float Temp_C;
float refTemp = 0;
float tempCoeff[3] = {0.05, 0.05, 0.05}; //deg/s per °C
float gyroBias[3] = {0, 0, 0};
bool gyroCalibrated = false;
float linAccFilt[3] = {0, 0, 0};
float alpha = 0.1; //accel low-pass filter
//Quaternion
float q[4] = {1, 0, 0, 0};
//Mahony filter parameters
float Kp = 20.0;
float Ki = 0.0;
float ix = 0, iy = 0, iz = 0;
//===rotary encoder variables
//(measures relative pos(not absolute like gps),
//so for error not to grow we do pos/time(aka velocity))====
volatile int cnt1 = 0;
volatile int lastcnt1 = 0;
volatile bool lastCLK1;
volatile int cnt2 = 0;
volatile int lastcnt2 = 0;
volatile bool lastCLK2;
unsigned long lastenctime = 0;
#define encinterval 100 //Calculate speed every 100ms
#define WHEELD 0.065 //wheel diameter
#define TRACKWIDTH 0.16 //meters between wheels
#define CIRCUMFERENCE PI*WHEELD
#define CNTPERREV 20
#define MPERCNT CIRCUMFERENCE/CNTPERREV //meters per count
//===Kalman filter variables====
//State
float x_pos = 0;
float x_vel = 0;
float x_bias = 0;
//Covariance
float P[3][3] = {
{1, 0, 0},
{0, 1, 0},
{0, 0, 1}
};
//yaw
float yaw = 0;
float yawRateBias = 0;
float P_yaw[2][2] = {
{1,0},
{0,1}
};
//Noise (TUNE THESE)
float accel_noise_std=0.5;//m/s^2
float encoder_noise_std=0.05;//meters/s
float bias_noise_std=0.01;//m/s^2 / sqrt(s)
float gyroYawNoise=0.05;
float encoderYawNoise=0.2;
float biasYawNoise=0.001;
//=======
unsigned long lastMicros;
//===========
void setup(){
Serial.begin(9600);
Wire.begin();
//IMU init
Wire.beginTransmission(MPU_ADDR);
Wire.write(0x6B);
Wire.write(0);
Wire.endTransmission(true);
calibrateGyro();
//Encoder
pinMode(CLK1, INPUT_PULLUP);
pinMode(DT1, INPUT_PULLUP);
lastCLK1 = digitalRead(CLK1);
pinMode(CLK2, INPUT_PULLUP);
pinMode(DT2, INPUT_PULLUP);
lastCLK2 = digitalRead(CLK2);
//=======TIMER1 SETUP (Polling every 1ms)=======
noInterrupts();
TCCR1A = 0;
TCCR1B = 0;
TCNT1 = 0;//initialize counter value to 0
//set compare match register for 1000Hz increments
//formula:(16MHz / (prescaler * desired_freq)) - 1
OCR1A = 249;// = (16,000,000 / (64 * 1000)) - 1
TCCR1B |= (1 << WGM12);//turn on CTC mode
TCCR1B |= (1 << CS11) | (1 << CS10); //set CS11 and CS10 bits for 64 prescaler
TIMSK1 |= (1 << OCIE1A);//enable timer compare interrupt
interrupts();//enable all interrupts
lastMicros = micros();
Serial.println("1D Kalman Filter: IMU + Encoder");
}
//=============
void loop(){
//=====Read IMU=======
Wire.beginTransmission(MPU_ADDR);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(MPU_ADDR, 14, true);
ax = Wire.read() << 8 | Wire.read();
ay = Wire.read() << 8 | Wire.read();
az = Wire.read() << 8 | Wire.read();
Temp_raw = Wire.read() << 8 | Wire.read();
gx = Wire.read() << 8 | Wire.read();
gy = Wire.read() << 8 | Wire.read();
gz = Wire.read() << 8 | Wire.read();
//=======Time step========
unsigned long tnow = micros();
float dt = (tnow - lastMicros) * 1e-6;
lastMicros = tnow;
//======Accel in g========
float ax_g = ax * ACCEL_SCALE;
float ay_g = ay * ACCEL_SCALE;
float az_g = az * ACCEL_SCALE;
//========Gyro rad/s=======
float gx_r = (gx - gyroBias[0]) * GYRO_SCALE * DEG_TO_RAD;
float gy_r = (gy - gyroBias[1]) * GYRO_SCALE * DEG_TO_RAD;
float gz_r = (gz - gyroBias[2]) * GYRO_SCALE * DEG_TO_RAD;
//=====temperature=====
Temp_C = (Temp_raw / 340.0) + 36.53;
float tempError = Temp_C - refTemp;
gx_r -= tempCoeff[0] * tempError * DEG_TO_RAD;
gy_r -= tempCoeff[1] * tempError * DEG_TO_RAD;
gz_r -= tempCoeff[2] * tempError * DEG_TO_RAD;
//=======Mahony========
MahonyUpdate(ax_g, ay_g, az_g, gx_r, gy_r, gz_r, dt);
//========Gravity========
float grav[3];
grav[0] = 2 * (q[1]*q[3] - q[0]*q[2]);
grav[1] = 2 * (q[0]*q[1] + q[2]*q[3]);
grav[2] = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];
//=======Linear acceleration========
float linAcc[3];
linAcc[0] = ax_g - grav[0];
linAcc[1] = ay_g - grav[1];
linAcc[2] = az_g - grav[2];
//linear acc filter
for(int i = 0; i < 3; i++){
linAccFilt[i] = alpha * linAcc[i] + (1 - alpha) * linAccFilt[i];
}
float ax_mps2 = linAccFilt[0] * 9.80665;
//=======Kalman predict(IMU)========
kalmanPredict(ax_mps2, dt);
yawPredict(gz_r, dt);
//======Kalman update/correct(encoder)=======
static unsigned long lastEncUpdate = 0;
if(millis() - lastEncUpdate > 100){//10Hz
lastEncUpdate = millis();
int c1=cnt1;
int c2=cnt2;
unsigned long now = millis();
if(now - lastenctime >= encinterval){//dont read speed to often
float enc1Vel=(float)((c1-lastcnt1)*MPERCNT)/((now-lastenctime)* 1e-3);
lastcnt1=c1;
float enc2Vel=(float)((c2-lastcnt2)*MPERCNT)/((now-lastenctime)* 1e-3);
lastcnt2=c2;
float omegaEnc=(enc2Vel-enc1Vel)/TRACKWIDTH;
static float yawEnc = 0;
yawEnc+=omegaEnc*((now-lastenctime)*1e-3);
//======Print=========
Serial.print("enc1Vel: ");
Serial.print(enc1Vel);
//Serial.print((float)encoderCount, 3);
Serial.print(" | KF POS: ");
Serial.print(x_pos, 3);
Serial.print(" | KF VEL: ");
Serial.print(x_vel, 3);
Serial.print(" | YAW: ");
Serial.println(yaw * RAD_TO_DEG);
float robotVel=(enc1Vel + enc2Vel) * 0.5f;
kalmanUpdateVelocity(robotVel);
yawUpdate(yawEnc);
lastenctime=now;
}
}
}
//===encoder ISR===
//timer interrupt to do polling of encoders
ISR(TIMER1_COMPA_vect){
//ENCODER 1
bool currentCLK1 = digitalRead(CLK1);
if(currentCLK1 != lastCLK1){
if(currentCLK1 == LOW){
if(digitalRead(DT1) != currentCLK1){
cnt1++;
} else{
cnt1--;
}
}
}
lastCLK1 = currentCLK1;
//ENCODER 2
bool currentCLK2 = digitalRead(CLK2);
if(currentCLK2 != lastCLK2){
if(currentCLK2 == LOW){
if(digitalRead(DT2) != currentCLK2){
cnt2++;
}
else{
cnt2--;
}
}
}
lastCLK2 = currentCLK2;
}
//====Kalman fcts=====
void kalmanPredict(float acc, float dt){
float dt2 = dt * dt;
//State prediction
x_pos += x_vel * dt + 0.5f * (acc - x_bias) * dt2;
x_vel += (acc - x_bias) * dt;
float A[3][3] = {
{1, dt, -0.5f * dt2},
{0, 1, -dt},
{0, 0, 1}
};
float AT[3][3] = {
{1, 0, 0},
{dt, 1, 0},
{-0.5f*dt2, -dt, 1}
};
//P = A*P*A'
float Pn[3][3] = {0};
for(int i=0;i<3;i++){
for(int j=0;j<3;j++){
for(int k=0;k<3;k++){
Pn[i][j] += A[i][k] * P[k][j];
}
}
}
for(int i=0;i<3;i++){
for(int j=0;j<3;j++) {
P[i][j] = 0;
for(int k=0;k<3;k++){
P[i][j] += Pn[i][k] * AT[k][j];
}
}
}
//Process noise Q = G*?²*G'
float G[3] = {0.5f * dt2, dt, 0.0f};
float var = accel_noise_std * accel_noise_std;
for(int i=0;i<3;i++){
for(int j=0;j<3;j++){
P[i][j] += var * G[i] * G[j];
}
}
//bias noise
P[2][2] += bias_noise_std * bias_noise_std * dt;
}
void yawPredict(float gyroZ, float dt){
yaw += (gyroZ - yawRateBias) * dt;
float A00 = 1;
float A01 = -dt;
float A10 = 0;
float A11 = 1;
float P00 = A00*P_yaw[0][0] + A01*P_yaw[1][0];
float P01 = A00*P_yaw[0][1] + A01*P_yaw[1][1];
float P10 = P_yaw[1][0];
float P11 = P_yaw[1][1];
P_yaw[0][0] = P00*A00 + P01*A01;
P_yaw[0][1] = P00*A10 + P01*A11;
P_yaw[1][0] = P10*A00 + P11*A01;
P_yaw[1][1] = P10*A10 + P11*A11;
P_yaw[0][0] += gyroYawNoise*gyroYawNoise*dt;
P_yaw[1][1] += biasYawNoise*biasYawNoise*dt;
}
/*void kalmanUpdate(float z){
float y = z - x_pos;
float S = P[0][0] + encoder_noise_std * encoder_noise_std;
float K[3];
K[0] = P[0][0] / S;
K[1] = P[1][0] / S;
K[2] = P[2][0] / S;
x_pos += K[0] * y;
x_vel += K[1] * y;
x_bias += K[2] * y;
float P_new[3][3];
for(int i=0;i<3;i++){
for(int j=0;j<3;j++){
P_new[i][j] = P[i][j] - K[i] * P[0][j];
}
}
memcpy(P, P_new, sizeof(P));
}*/
void kalmanUpdateVelocity(float z){
//Measurement: z = velocity
float y = z - x_vel;
float R = encoder_noise_std * encoder_noise_std;
float S = P[1][1] + R;
float K[3];
K[0] = P[0][1] / S;
K[1] = P[1][1] / S;
K[2] = P[2][1] / S;
//State update
x_pos += K[0] * y;
x_vel += K[1] * y;
x_bias += K[2] * y;
//Covariance update: P = (I - K H) P
//H = [0 1 0]
float P_new[3][3];
for(int i = 0; i < 3; i++){
for(int j = 0; j < 3; j++){
P_new[i][j] = P[i][j] - K[i] * P[1][j];
}
}
memcpy(P, P_new, sizeof(P));//copy memory block(dest, source, size)
}
void yawUpdate(float yawEnc){
float y = yawEnc - yaw;
float S = P_yaw[0][0] + encoderYawNoise*encoderYawNoise;
float K0 = P_yaw[0][0] / S;
float K1 = P_yaw[1][0] / S;
yaw += K0 * y;
yawRateBias += K1 * y;
float P00 = P_yaw[0][0];
float P01 = P_yaw[0][1];
P_yaw[0][0] -= K0 * P00;
P_yaw[0][1] -= K0 * P01;
P_yaw[1][0] -= K1 * P00;
P_yaw[1][1] -= K1 * P01;
}
//=======calibration=======
void calibrateGyro(){
const int samples = 500;
long sum[3] = {0, 0, 0};
int16_t tempRaw=0;
Serial.println("Calibrating gyro... keep IMU still");
for(int i = 0; i < samples; i++){
Wire.beginTransmission(MPU_ADDR);
Wire.write(0x41); //start at TEMP_OUT_H
Wire.endTransmission(false);
Wire.requestFrom(MPU_ADDR, 8, true);
tempRaw = Wire.read() << 8 | Wire.read();
int16_t gx = Wire.read() << 8 | Wire.read();
int16_t gy = Wire.read() << 8 | Wire.read();
int16_t gz = Wire.read() << 8 | Wire.read();
sum[0] += gx;
sum[1] += gy;
sum[2] += gz;
delay(5);
}
gyroBias[0] = sum[0] / (float)samples;
gyroBias[1] = sum[1] / (float)samples;
gyroBias[2] = sum[2] / (float)samples;
gyroCalibrated = true;
//get reference temperature
refTemp = (tempRaw / 340.0) + 36.53;
Serial.print("temp:");
Serial.println(refTemp);
Serial.println("Gyro and temp calibration done");
}
//===Mahony filter====
void MahonyUpdate(float ax, float ay, float az, float gx, float gy, float gz, float dt){
float recipNorm;
float vx, vy, vz;
float ex, ey, ez;
float q0=q[0], q1=q[1], q2=q[2], q3=q[3];
float norm = ax*ax + ay*ay + az*az;
if(norm > 0){
recipNorm = 1.0 / sqrt(norm);
ax *= recipNorm; ay *= recipNorm; az *= recipNorm;
vx = 2*(q1*q3 - q0*q2);
vy = 2*(q0*q1 + q2*q3);
vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
ex = (ay*vz - az*vy);
ey = (az*vx - ax*vz);
ez = (ax*vy - ay*vx);
gx += Kp * ex;
gy += Kp * ey;
gz += Kp * ez;
}
gx *= 0.5 * dt;
gy *= 0.5 * dt;
gz *= 0.5 * dt;
q[0] += (-q1*gx - q2*gy - q3*gz);
q[1] += ( q0*gx + q2*gz - q3*gy);
q[2] += ( q0*gy - q1*gz + q3*gx);
q[3] += ( q0*gz + q1*gy - q2*gx);
recipNorm = 1.0 / sqrt(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
q[0]*=recipNorm; q[1]*=recipNorm;
q[2]*=recipNorm; q[3]*=recipNorm;
}
Im happy with how little drift there is(at least in the 5 ish min I ran the code), the only thing is it overly trusts the encoders because the MPU6050 is veeeerrrryyyy veeerrryyyyy noisy so these Arduino codes cannot deal with wheel slip. I tried inflating R if wheel slip is detected but the IMU is so noisy that the if statment to detect it never triggers properly. But otherwise it works great so it's good enough for me. Thats the beauty of the Kalman filter, that you can tune how much you trust each sensor.
Copyright © 2026, Jessica Socher ()