duration=60;

dt=0.1;

MeasNoise = 10;

accelnoise = 0.2;

a = [1 dt 0; 0 1 0; 0 0 1];

b = [dt^2/2 0; dt 0; 0 dt];

c = [1 0 0; 0 0 1];

x = [0; 0; 0];

xhat = x;

Sz = [MeasNoise^2 0; 0 MeasNoise^2];

Sw = [10^-6 0 0; 0 4*10^-4 0; 0 0 0.05];

P = Sw;

pos = [];

poshat = [];

posmeas = [];

vel = [];

velhat = [];

angle = [];

anglehat = [];

anglemeas = [];

for t = 0 : dt: duration,

u = [1;4*dt];

ProcessNoise = accelnoise * [(dt^2/2)*randn; dt*randn; dt*randn];

x = a * x + b * u + ProcessNoise;

MeasNoise = [0.5*randn; 0.5*randn]

y = c * x + MeasNoise;

xhat = a * xhat + b * u;

Inn = y - c * xhat;

s = c * P * c' + Sz;

K = a * P * c' * inv(s);

xhat = xhat + K * Inn;

P = a * P * a' - a * P * c' * inv(s) * c * P * a' + Sw;

pos = [pos; x(1)];

posmeas = [posmeas; y(1)];

poshat = [poshat; xhat(1)];

vel = [vel; x(2)];

velhat = [velhat; xhat(2)];

angle = [angle; x(3)];

anglemeas = [anglemeas; y(2)];

anglehat = [anglehat; xhat(3)];

end

close all;

t = 0 : dt : duration;

figure;

plot(t,pos, t,posmeas, t,poshat);

grid;

xlabel('Time (sec)');

ylabel('Position (feet)');

title('Figure 1 - Vehicle Position (True, Measured, and Estimated)')

figure;

plot(t,angle, t,anglemeas, t,anglehat);

grid;

xlabel('Time (sec)');

ylabel('angle');

title('Figure 1 - Vehicle orientation (True, Measured, and Estimated)')

figure;

plot(t,pos-posmeas, t,pos-poshat);

grid;

xlabel('Time (sec)');

ylabel('Position Error (feet)');

title('Figure 2 - Position Measurement Error and Position Estimation Error');

figure;

plot(t,vel, t,velhat);

grid;

xlabel('Time (sec)');

ylabel('Velocity (feet/sec)');

title('Figure 3 - Velocity (True and Estimated)');

figure;

plot(t,vel-velhat);

grid;

xlabel('Time (sec)');

ylabel('Velocity Error (feet/sec)');

title('Figure 4 - Velocity Estimation Error');

## 3 Comments

## Direct link to this comment

https://www.mathworks.com/matlabcentral/answers/1774-converting-m-file-to-c-code#comment_3362

⋮## Direct link to this comment

https://www.mathworks.com/matlabcentral/answers/1774-converting-m-file-to-c-code#comment_3362

## Direct link to this comment

https://www.mathworks.com/matlabcentral/answers/1774-converting-m-file-to-c-code#comment_124367

⋮## Direct link to this comment

https://www.mathworks.com/matlabcentral/answers/1774-converting-m-file-to-c-code#comment_124367

## Direct link to this comment

https://www.mathworks.com/matlabcentral/answers/1774-converting-m-file-to-c-code#comment_124377

⋮## Direct link to this comment

https://www.mathworks.com/matlabcentral/answers/1774-converting-m-file-to-c-code#comment_124377

Sign in to comment.