Kalman filter for FPGA in HDL Coder?

Hello, I am currently trying to make the kalman filter verilog code using HDL coder on MATLAB, to convert the MATLAB code into the Verilog/VHDL code.

To do this, I have to re-formulate my code into the format that the HDL coder accept.

My original code goes:

```

mass = 10^-12; % [kg]

dt = 10^-5; % Step time [s]

Nt = 10 / dt; % Number of time steps

Ts = (0:dt:Nt*dt); % Time plot

Omegas = 2*pi* [100 10 10]; % Spring frequnency; Testing 3 DOFs at maximum

Gammas = 2*pi* [0.1 0.1 1]; % Damping rate; Testing 3 DOFs at maximum

DOFs = 3; % Degrees of freedom; 1, 2, 3

Order = 3; % Orders for Exponential

% System matrix

A = zeros(2*DOFs,2*DOFs);

for dof = 1:DOFs

A(2*dof-1, 2*dof) = 1;

A(2*dof, 2*dof-1) = -Omegas(dof)^2;

A(2*dof, 2*dof) = -Gammas(dof);

end

% External force

kb = 1.38 * 10^(-23); % Boltzmann constant

T = 298;

% Var_sys = sqrt(2*mass*kb*T*Gammas); % System state noise due to thermal flucuations

Var_sys = sqrt(2*mass*kb*T*Gammas*10^10); % System state noise due to thermal flucuations

Var_measure = 0.0081*ones(1,3); % Measurement noise variance

fext_t = zeros(2*DOFs, length(Ts)); % external force divided by mass

% %{

% Ex.1. Parametric resonance

for dof = 1:DOFs

fext_t(2*dof,:) = 1*cos(Omegas(dof)*Ts);

end

rhs = @(t,x) A*x + 1*[0; cos(Omegas(1)*t); 0; cos(Omegas(2)*t); 0; cos(Omegas(3)*t)]; % rhs of function

xinit = repmat([1;0],DOFs,1); % initial value

[~,trueTrajectory] = ode45(rhs,Ts,xinit);

%}

% Display simulation conditions

fprintf('Simulation time = %.1f seconds, Bandwidth = %.1f kHz\n', Nt*dt, 1/dt/1000);

fprintf('Spectral radius of dt * A = %.5f.\n\n', abs(det(A))^(1/(2*DOFs))*dt);

for dof = 1:DOFs

fprintf(['%.0fst dimension: Spring frequency = %.1f Hz, damping rate = ' ...

'%.1f Hz \n'], dof, Omegas(dof)/2/pi, Gammas(dof)/2/pi);

end

rng;

noise_measure = sqrt(Var_measure).*randn(length(Ts),DOFs);

Trajectory_measureNoise = trueTrajectory(:,1:2:end) + noise_measure; %Add noise

% 2. Add system noise too

Trajectory_totalNoise = zeros(2*DOFs, length(Ts));

F = eye(2*DOFs);

for n = 2:Order

F = F+dt^(n-1)/factorial(n-1) * A^(n-1); % System propagation function

end

B = inv(A) * (F-eye(2*DOFs));

% Solve the time-dependent equation - Without force

Trajectory_totalNoise(:) = 0;

Trajectory_totalNoise(:,1) = xinit;

for tt = 2:size(Trajectory_totalNoise,2)

Trajectory_totalNoise(:,tt) = F*Trajectory_totalNoise(:,tt-1) + B * fext_t(:,tt);

Trajectory_totalNoise(1:2:end,tt) = Trajectory_totalNoise(1:2:end,tt) + sqrt(Var_sys)'.*randn(DOFs,1);

end

Trajectory_totalNoise(1:2:end,:) = Trajectory_totalNoise(1:2:end,:) +noise_measure';

% Trajectory_totalNoise = Trajectory_totalNoise(1:2:end,:);

figure,

subplot(311),

plot(Ts, Trajectory_totalNoise(1,:), 'color', [1 0 0 1], 'linewidth', 2), hold on

plot(Ts, Trajectory_measureNoise(:,1), 'color',[0.2 0.2 0.2 0.3], 'linewidth', 2), hold on

xlabel('Time (s)','fontname','Arial Nova Cond','FontSize', 10),

ylabel('Amplitude','fontname','Arial Nova Cond','FontSize', 10)

legend('x')

set(gcf,'color','w')

subplot(312),

plot(Ts, Trajectory_totalNoise(3,:), 'color', [0 1 0 1], 'linewidth', 2), hold on

plot(Ts, Trajectory_measureNoise(:,2), 'color',[0.2 0.2 0.2 0.3], 'linewidth', 2), hold on

xlabel('Time (s)','fontname','Arial Nova Cond','FontSize', 10),

ylabel('Amplitude','fontname','Arial Nova Cond','FontSize', 10)

legend('x')

set(gcf,'color','w')

subplot(313),

plot(Ts, Trajectory_totalNoise(5,:), 'color', [0 0 1 1], 'linewidth', 2), hold on

plot(Ts, Trajectory_measureNoise(:,3), 'color',[0.2 0.2 0.2 0.3], 'linewidth', 2), hold on

xlabel('Time (s)','fontname','Arial Nova Cond','FontSize', 10),

ylabel('Amplitude','fontname','Arial Nova Cond','FontSize', 10)

legend('x')

set(gcf,'color','w')

%% Kalman filter

C = eye(2*DOFs);

Q = repmat(Var_sys,2,1);

Q = diag(Q(:));% Noise on system, e.g. thermal noise

R = repmat(Var_measure,2,1);

R = diag(R(:));% Noise on measurement

% Initialize state estimate & covariance estimate

Trajectory_filtered = Trajectory_totalNoise*0;

x_jj = xinit; % init value

Trajectory_filtered(:,1) = x_jj;

Cov_jj = Q;

% Kalman filtering

for tt = 2:size(Trajectory_totalNoise,2)

% 1. Prediction

x_Jj = F * x_jj + B * fext_t(:,tt);

Cov_Jj = F * Cov_jj * F' + Q;

% 2. Observation and update

y_J = Trajectory_totalNoise(:,tt);

K_J = (Cov_Jj * C')/(C*Cov_Jj*C'+R);

x_jj = x_Jj + K_J* (y_J - C*x_Jj);

Cov_jj = (eye(2*DOFs) - K_J*C)*Cov_Jj;

Trajectory_filtered(:,tt) = x_jj;

end

%% .... more code

I found this example on the MATLAB website :

https://www.mathworks.com/help/hdlcoder/gs/basic-hdl-code-generation-with-the-workflow-advisor.html

[ProblemSolver]

@Jingyi Li -- So what is the problem? What are you looking for?

Answer 1

[Kiran Kintali]

You need to break the MATLAB code into design and testbench and use MATLAB to HDL code advisor. See the sample example below.

>> mlhdlc_demo_setup('kalman')