1 Unstar Star 1 Fork 1

Daitole / demo_for_kalmanFilterMatlab

Create your Gitee Account
Explore and code with more than 5 million developers,Free private repositories !:)
Sign up
This repository doesn't specify license. Without author's permission, this code is only for learning and cannot be used for other purposes.
fusion algorithm with only ranging(UWB) and 6-axis imu sensor spread retract

Clone or download
demo_ekf_error.m 2.82 KB
Copy Edit Web IDE Raw Blame History
oyg5285 authored 2019-01-31 09:25 . rewrite code
%% Author : Gao Ouyang
%% Date : 2017.01.18
%% Descriptor: 5 UWB anchor(only ranging) and 6-axis MEMS-IMU
%% apply a ekf demo for 3-D Location, please see ReadMe.docx in
%% detail. the precision of ekf is less than that in ukf.
%% states : position,velocity,attitude,accel_bias,gyro_bias
%% measures: ranging (unit: m)
%% controls: accel , gyro(unit:deg/s)
%%
run fusion_init.m
%% Motion Process, Measurement model and it's derivative
f_func = @ekf_ins_f;
df_dx_func = @ekf_err_ins_f;
h_func = @uwb_h;
dh_dx_func = @err_uwb_h;
imu_iter = 1;
uwb_iter = 1;
dt = 0.02;
ImuPcs = length(SampleTimePoint);
ISPUTCHAR = 0;
% Reserve space for estimates.
StateByFilter = zeros(size(X,1),ImuPcs);
StateCoVariance = zeros(size(X,1),size(X,1),ImuPcs);
Invation = zeros(5,ImuPcs);
TrajectoryCollector= []; noise=[];
% Estimate with EKF
for k=1:ImuPcs
uk = [a_vector(imu_iter,:)';g_vector(imu_iter,:)'/180*pi];
% EKF
uk = uk + dX(10:15);
[~,E,D] = df_dx_func(X,[dt;uk(1:3);X(7:9)]);
X(1:9) = f_func(X,[dt;uk;X(7:9)]);
P = E*P*E' + D*Q*D';
if ISPUTCHAR == 1
cprintf('text', 'time: %8.3f s, Position = [%0.2f %0.2f %0.2f] m, Velocity = [%0.3f %0.3f %0.3f] m/s Position Variance = [%0.5f %0.5f %0.5f], Velocity Variance = [%0.5f %0.5f %0.5f]m/s^2\n',...
SampleTimePoint(imu_iter) ,X(1),X(2),X(3),X(4),X(5),X(6), P(1,1),P(2,2),P(3,3),P(4,4),P(5,5),P(6,6));
end
%%% Update
if uwb_iter <= length(UWBBroadTime_vector) && UWBBroadTime_vector(uwb_iter)== SampleTimePoint(imu_iter)
Z_meas = Uwbranging_vector(uwb_iter,:)';
%%%add the outlier noise
outlier = zero_one_distribution(0.1)';
%outlier = zeros(5,1); %% you could ignore annotation without outlier
Z_meas = Z_meas + outlier;
noise = [noise,outlier];
[~,H] = dh_dx_func(X);
%% 5 Anchor
K = P*H'*inv(H*P*H'+R);
dX = K*(Z_meas - h_func(X));Invation(:,k) = Z_meas - h_func(X);
P = P - K*H*P;
%%% feedback
X = X + dX;
if ISPUTCHAR == 1
cprintf('err', 'time: %8.3f s, Position [%0.2f %0.2f %0.2f] m, Velocity [%0.3f %0.3f %0.3f] m/s Position Variance = [%0.5f %0.5f %0.5f], Velocity Variance = [%0.5f %0.5f %0.5f]m/s^2\n\n\n',...
UWBBroadTime_vector(uwb_iter) ,X(1),X(2),X(3),X(4),X(5),X(6), P(1,1),P(2,2),P(3,3),P(4,4),P(5,5),P(6,6));
end
uwb_iter = uwb_iter + 1;
end
StateByFilter(:,k) = X;
StateCoVariance(:,:,k) = P;
imu_iter = imu_iter + 1;
end
StateByFilter(7:9,:)= StateByFilter(7:9,:)/pi*180;
noise = noise';
for uwb_iter=1:4:length(UWBBroadTime_vector)-10
Z_meas = diag(Uwbranging_vector(uwb_iter:uwb_iter+4,:) + noise(uwb_iter:uwb_iter+4,:)) ;
uwbxyz = triangulate(Z_meas);
TrajectoryCollector = [TrajectoryCollector,[UWBBroadTime_vector(uwb_iter+2);uwbxyz;TraceData(4*(uwb_iter+2)+1,2:4)']];
end
run fusion_display.m

Comment ( 0 )

Sign in for post a comment

Matlab
1
https://gitee.com/daitole/demo_for_kalmanFilter.git
git@gitee.com:daitole/demo_for_kalmanFilter.git
daitole
demo_for_kalmanFilter
demo_for_kalmanFilter
master

Search

132457 8cb2edc1 1899542 131848 70c8d3a4 1899542