# kalman filter constant accelaration model

i am trying to understand the kalman filter constant acceleration model(CA) through simulations but the results are not as expected but when i did the simulation in similar way for CV model i got good results however transition to CA has not been easy so i am trying to understand where i am going wrong

i have 3 measurements obtained from radar: range(r), azimuth(θ), radial velocity(v) i am converting them to x,y,vx and vy using the equations

$$x = rcosθ\\ y = rsinθ\\ vx = vcosθ\\ vy = vsinθ$$

my state matrix is

$$\text{CA_FILTX} = \begin{bmatrix}x \\y \\vx \\vy \\ax \\ay\end{bmatrix}$$

and observation matrix $$\text{H} = \begin{bmatrix}1 & 0 & 0 & 0 & 0 & 0\\0 & 1 & 0 & 0 & 0 & 0\\0 & 0 & 1 & 0 & 0 & 0\\0 & 0 & 0 & 1 & 0 & 0\end{bmatrix}$$

state transition matrix $$\text{CA_PHI} = \begin{bmatrix}1 & 0 & T & 0 & \frac{T^2}{2} & 0\\0 & 1 & 0 & T & 0 & \frac{T^2}{2}\\0 & 0 & 1 & 0 & T & 0\\0 & 0 & 0 & 1 & 0 & T\\0 & 0 & 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 0 & 0 & 1\end{bmatrix}$$

process noise covariance matrix

$$\text{Q} = \begin{bmatrix} \frac{T^5}{20} & 0 & \frac{T^4}{8} & 0 & \frac{T^3}{6} & 0 \\ 0 & \frac{T^5}{20} & 0 & \frac{T^4}{8} & 0 & \frac{T^3}{6} \\ \frac{T^4}{8} & 0 & \frac{T^3}{3} & 0 & \frac{T^2}{2} & 0 \\ 0 & \frac{T^4}{8} & 0 & \frac{T^3}{3} & 0 & \frac{T^2}{2} \\ \frac{T^3}{6} & 0 & \frac{T^2}{2} & 0 & T & 0 \\ 0 & \frac{T^3}{6} & 0 & \frac{T^2}{2} & 0 & T \end{bmatrix}$$

and the measurement noise covariance matrix in polar where Sigma_Rng,Sigma_Azi and Sigma_vel are standard deviation of measurement error $$\text{Sigma_matrix} = \begin{bmatrix} \text{Sigma_Rng}^2 & 0 & 0 \\ 0 & \text{Sigma_Azi}^2 & 0 \\ 0 & 0 & \text{Sigma_vel}^2 \end{bmatrix}$$

i am calculating the mesurement noise covariance matrix R with the help of jacobian matrix

$$\text{R} = \text{jacobian_matrix} \cdot \text{Sigma_matrix} \cdot \text{jacobian_matrix}^T$$ the jacobian matrix i am using is

$$\text{jacobian_matrix} = \begin{bmatrix} \cosθ & -\text{r} \sinθ & 0 \\ \sinθ & \text{r} \cosθ & 0 \\ 0 & -\text{v} \sin\theta & \cos\theta \\ 0 & \text{v} \cos\theta & \sin\theta \end{bmatrix}$$

and i am using the fallowing equation to estimate my states $$\text{CA_PRED} = \text{CA_PHI} \cdot \text{CA_FILTX}$$ $$\text{CA_PRED_COV} = \left( \text{CA_PHI} \cdot \text{CA_FILTX_COV} \cdot \text{CA_PHI}^T \right) + Q$$

$$\text{meas_matrix} = \begin{bmatrix}x \\y \\vx \\vy \end{bmatrix}$$ $$\text{CA_innovation} = \text{meas_matrix} - (H \cdot \text{CA_PRED})$$ $$\text{S} = (H \cdot \text{CA_PRED_COV} \cdot H^T) + R$$ $$\text{K} = \text{CA_PRED_COV} \cdot H^T \cdot S^{-1}$$ $$\text{CA_FILTX} = \text{CA_PRED}\ + (\text{K} \cdot \text{CA_innovation})$$ $$\text{CA_FILTX_COV} = \text{CA_PRED_COV} - (K \cdot H \cdot \text{CA_PRED_COV})$$

my simulated results are this way

but i expected my acceleration to become constant after some time so here are my list of doubts

1. the matrices i am using are they correct
2. am i taking care of the noise correctly?
3. the jacobian matrix i used is it correct?

as per the request i am attaching my matlab code here

%section 1: generating measurements
start_range = 2000;
azimuth = 30;
v = 2;
accel = 0.1;
deltaT = 1;
total_duration = 100;
tgttype = 'incoming';

time_vec = 0:deltaT:total_duration;
num_samples = length(time_vec);

sigma_range = 3;
sigma_azimuth = 0.05;
sigma_velocity = 0.5;

range_noise = normrnd(0, sigma_range, [1, num_samples]);
azimuth_noise = normrnd(0, sigma_azimuth, [1, num_samples]);
velocity_noise = normrnd(0, sigma_velocity, [1, num_samples]);

true_range = zeros(1, num_samples);
true_azimuth = zeros(1, num_samples);
true_vel = zeros(1, num_samples);
meas_range = zeros(1, num_samples);
meas_azimuth = zeros(1, num_samples);
meas_vel = zeros(1, num_samples);

truefid = fopen("CA_trueval.txt","w");
measfid = fopen("CA_measurements.txt","w");

true_range(1) = start_range;
true_azimuth(1) = azimuth;
true_vel(1) = v;
fprintf(truefid,"%d  %f  %f  %f %f\n",1,true_range(1),true_azimuth(1),true_vel(1),accel);

for i = 2:num_samples
true_vel(i) = v + (accel*time_vec(i));

if tgttype == 'outgoing'
true_range(i) = start_range + (true_vel(i)*time_vec(i));
elseif tgttype == 'incoming'
true_range(i) = start_range - (true_vel(i)*time_vec(i));
end
true_azimuth(i) = azimuth;
fprintf(truefid,"%d  %f  %f  %f %f\n",i,true_range(i),true_azimuth(i),true_vel(i),accel);
endfor

% Adding noise to true measurements
meas_range = true_range + range_noise;
meas_azimuth = true_azimuth + azimuth_noise;
meas_vel = true_vel + velocity_noise;

for j = 1:num_samples
fprintf(measfid,"%d  %f  %f  %f\n",j,meas_range(j),meas_azimuth(j),meas_vel(j));
end
fclose(measfid);
fclose(truefid);

%section 2: kalman filter part
predfid = fopen("CA_Pred.txt","w");
filtfid = fopen("CA_filt.txt","w");

fprintf(predfid,"%d %f  %f  %f %f\n",1,0,0,0,0);
fprintf(filtfid,"%d %f  %f  %f %f\n",1,0,0,0,0);

T = deltaT;

T2 = T^2;
T3 = T^3;
T4 = T^4;
T5 = T^5;

%observation matrix
H = [ 1 0 0 0 0 0
0 1 0 0 0 0
0 0 1 0 0 0
0 0 0 1 0 0];

%state transition matrix
CA_PHI = [1   0   T   0   T2/2  0
0   1   0   T   0     T2/2
0   0   1   0   T     0
0   0   0   1   0     T
0   0   0   0   1     0
0   0   0   0   0     1];

% process noise covariance matrix
Q = [T5/20   0       T4/8    0       T3/6    0
0       T5/20   0       T4/8    0       T3/6
T4/8    0       T3/3    0       T2/2    0
0       T4/8    0       T3/3    0       T2/2
T3/6    0       T2/2    0       T       0
0       T3/6    0       T2/2    0       T];

%measurement noise covariance polar
Sigma_matrix = [ sigma_range^2    0                 0
0              sigma_azimuth^2   0
0              0                 sigma_velocity^2];

rng = measured_data(1,2);
az = measured_data(1,3);
v = measured_data(1,4);
accel = (measured_data(2,4) - measured_data(1,4)) / deltaT

%state matrix
CA_FILTX = [x
y
vx
vy
ax
ay];

%intial filter covariance (taking identity matrix)
CA_FILTX_COV = eye(6);

for i = 2:num_samples

CA_PRED = CA_PHI * CA_FILTX;
CA_PRED_COV = (CA_PHI * CA_FILTX_COV * CA_PHI') + Q;

range = measured_data(i,2);
azi = measured_data(i,3);
vel = measured_data(i,4);

%measurement noise covariance cartesian
R = jacobian_matrix * Sigma_matrix * jacobian_matrix';

meas_matrix = [new_x
new_y
new_vx
new_vy];

CA_innovation = meas_matrix - (H * CA_PRED);
S = (H * CA_PRED_COV * H') + R;
K = CA_PRED_COV * H' * inv(S);

CA_FILTX = CA_PRED + (K * CA_innovation);
CA_FILTX_COV = CA_PRED_COV - (K * H * CA_PRED_COV);

pred_range = sqrt((CA_PRED(1,1)*CA_PRED(1,1)) + (CA_PRED(2,1)*CA_PRED(2,1)));
pred_vel = sqrt((CA_PRED(3,1)*CA_PRED(3,1)) + (CA_PRED(4,1)*CA_PRED(4,1)));
pred_accel = sqrt((CA_PRED(5,1)*CA_PRED(5,1)) + (CA_PRED(6,1)*CA_PRED(6,1)));

filt_range = sqrt((CA_FILTX(1,1)*CA_FILTX(1,1)) + (CA_FILTX(2,1)*CA_FILTX(2,1)));
filt_vel = sqrt((CA_FILTX(3,1)*CA_FILTX(3,1)) + (CA_FILTX(4,1)*CA_FILTX(4,1)));
filt_accel = sqrt((CA_FILTX(5,1)*CA_FILTX(5,1)) + (CA_FILTX(6,1)*CA_FILTX(6,1)));

fprintf(predfid,"%d %f  %f  %f %f\n",i,pred_range,pred_azi,pred_vel,pred_accel);
fprintf(filtfid,"%d %f  %f  %f %f\n",i,filt_range,filt_azi,filt_vel,filt_accel);
endfor
fclose(predfid);
fclose(filtfid);

meas_rng = measured_data(:,2);
meas_azi = measured_data(:,3);
meas_vel = measured_data(:,4);

prd_rng = pred_data(:,2);
prd_azi = pred_data(:,3);
prd_vel = pred_data(:,4);
prd_accel = pred_data(:,5);

true_rng = true_data(:,2);
true_azi = true_data(:,3);
true_vel = true_data(:,4);
true_accel = true_data(:,5);

flt_rng = filter_data(:,2);
flt_azi = filter_data(:,3);
flt_vel = filter_data(:,4);
flt_accel = filter_data(:,5);

figure;
plot(true_rng,'go-',meas_rng,'ko-', prd_rng,'bo-',flt_rng,'ro-');
title('CA model Range');
legend('true range','Measured Range','pred range','filt range');

figure;
plot(true_azi,'go-',meas_azi,'ko-',prd_azi,'bo-',flt_azi,'ro-');
title('CA model Azimuth ');
legend('True Azimuth', 'Measured Azimuth','pred azimuth','filt azi');

figure;
plot(true_vel,'go-',meas_vel,'ko-',prd_vel,'bo-',flt_vel,'ro-');
title('CA model Velocity ');
legend('True Velocity','measured vel','pred velocity','filter vel');

figure;
plot(true_accel,'go-',prd_accel,'bo-',flt_accel,'ro-');
title('CA model Accelaration ');
legend('True accelaration','pred accelaration','filter accelaration');


i hope i have given clear picture of my problem please help me understand what is the issue and how do i solve it. Any suggestion/help will be very much appreciated,looking forward to learn more about kalman filter. Thanks in advance for any help

New contributor
kms is a new contributor to this site. Take care in asking for clarification, commenting, and answering. Check out our Code of Conduct.
• Can you write out your equations using Latex? Commented Jul 30 at 19:44
• @Baddioes i have edited the question and wrote my equation using Latex
– kms
Commented Jul 31 at 12:24
• Can you post your data files, and the whole code so I can reproduce the results myself and try some things out? Commented Jul 31 at 20:48
• @Baddioes thanks for the reply i have posted my full code here and the data files will be generated when you run the code so i have not provided them separately please do check and let me know if anything else is required from my end
– kms
Commented Aug 1 at 5:21
• Just verifying, this isn't a homework question, is it? Commented 2 days ago

I'll explain this in more general terms as it'll be easier. Let's assume we have true target state $$\underline{x}$$, estimated target state $$\underline{\hat{x}}$$, measurement $$\underline{z}$$, state transition matrix $$\mathbf{F}$$, and observation matrix $$\mathbf{H}$$. In real world scenarios, all that's needed is the Kalman loop because $$\underline{z}$$ is generated from actually collected data. But in simulation land, the data has to be simulated according to the linear data model. So first you want to simulate all your data according to the state-space equations, given by \begin{align} \underline{z}_{k} &= \mathbf{H}\underline{x}_{k}\\ \underline{x}_{k+1} &= \mathbf{F}\underline{x}_{k} \end{align}

Then, we set $$\hat{\underline{x}}_{0|-1} = \underline{x}_{0} + \underline{w}$$, ie, the initial prediction is some guess relatively in the range of the true values. For reference, the notation $$\underline{y}_{k|k}$$ is the filtered value at time $$k$$, and the notation $$\underline{y}_{k|k-1}$$ is the predicted value at time $$k$$ from the previous $$k-1$$ samples. From here, we can go through the Kalman loop \begin{align} \underline{\epsilon}_{k} &= \underline{z}_{k} - \mathbf{H}\underline{\hat{x}}_{k|k-1} \\ \mathbf{S} &= \mathbf{H}\mathbf{P}_{k|k-1}\mathbf{H}^{T} + \mathbf{R} \\ \mathbf{K} &= \mathbf{P}_{k|k-1}\mathbf{H}\mathbf{S}^{-1} \\ \underline{\hat{x}}_{k|k} &= \underline{\hat{x}}_{k|k-1} + \mathbf{K}\underline{\epsilon}_{k} \\ \mathbf{P}_{k|k} &= \left(\mathbf{I} - \mathbf{K}\mathbf{H}\right)\mathbf{P}_{k|k-1} \\ \underline{\hat{x}}_{k+1|k} &= \mathbf{F}\underline{\hat{x}}_{k|k} \\ \mathbf{P}_{k+1|k} &= \mathbf{F}\mathbf{P}_{k|k}\mathbf{F}^{T} + \mathbf{Q} \end{align}

EDIT: Since this isn't a homework question, below is the code I posted to generate the data. It may not all be right, but it gives something better.

  % ADDED MY CODE
X = zeros(size(CA_FILTX_COV,1),num_samples);
Y = zeros(size(H,1),num_samples);
X(:,1) = [start_range*cosd(azimuth); start_range*sind(azimuth); v*cosd(azimuth); v*sind(azimuth); accel*cosd(azimuth); accel*sind(azimuth)];
for ii = 1:num_samples
ang(ii) = atand(X(2,ii)/X(1,ii));
r(ii) = sqrt(X(1,ii).^2+X(2,ii).^2);
vel(ii) = sqrt(X(3,ii).^2+X(4,ii).^2);
Y(:,ii) = H*X(:,ii) + J*Sigma_matrix*randn(3,1); % Measurement
if ii ~= num_samples
X(:,ii+1) = CA_PHI*X(:,ii) + [randn(size(CA_PHI,1)-2,1); 0; 0]; % State Transition
end
end

Xpred = zeros(size(X));
Xpred(:,1) = CA_FILTX + randn(size(CA_FILTX));
Xfilt = zeros(size(Xpred));
E = zeros(size(Y));

for ii = 1:num_samples
% Xf = CA_PHI*Xfilt(:,ii-1);
ang(ii) = atand(Y(2,ii)/Y(1,ii));
r(ii) = sqrt(Y(1,ii).^2+Y(2,ii).^2);
vel(ii) = sqrt(Y(3,ii).^2+Y(4,ii).^2);

E(:,ii) = Y(:,ii) - H*Xpred(:,ii);

S = H*CA_PRED_COV*(H.') + J*Sigma_matrix*(J.');

K = CA_PRED_COV*(H.')*inv(S);

Xfilt(:,ii) = Xpred(:,ii) + K*E(:,ii);
CA_PRED_COV = (eye(size(CA_PRED_COV,1))-K*H)*CA_PRED_COV;

if ii ~= num_samples
Xpred(:,ii+1) = CA_PHI*Xfilt(:,ii);
CA_PRED_COV = CA_PHI*CA_PRED_COV*(CA_PHI.') + Q;
end
end

• so the problem is at the generation of my measurement data
– kms
Commented 2 days ago
• @kms correct, you have to generate the data according to the state space equations, otherwise you might violate the assumptions of the model. If you look at the other comment, your time derivatives might be wrong, but other than that you are probably good to go. Also, see my updated answer. Commented 2 days ago
• okay that makes more sense since my data is not suitable for testing the model it is not behaving the way i am expecting i didn't knew the data should be according to the state space equations thank you so much i will check this and come back again if i have any more doubt
– kms
Commented 2 days ago
• but while testing the CV model also i generated the measurements similar way but the filter results were good why this problem i am encountering in CA model
– kms
Commented 2 days ago
• i generated my measurements as per your suggestions there is a lot of improvement in the filter output i need to fine tune a bit but this gave me little confidence boost so thank you so much
– kms
Commented 2 days ago