# Even more on: Kalman filter for position and velocity

As a follow-on to this great question...

I'm trying to implement the model in the referenced article (and referenced Wikipedia article), but I'm not getting the results I think I should be getting. Ultimate I would like to take this example and marry it with intermittent GPS data so as to simulate accelerometer failover in situations where GPS is lost.

I've build the following model in Simulink:

UPDATED MODEL:

and power it using the following UPDATED Matlab code:

% Configure simulation
sim_length=10;
Ts=0.1;

% State space equations
A = [1 Ts;0 1];
C = [1 0];

% Noise characteristics
sigma_a = .1;
process_noise_power = 10;
measurement_noise_power = 10;

% Noise matrices
G = [(Ts^2)/2; Ts];
H = [1];

% Process noise
Q = sigma_a^2;
% Q = G*G'*sigma_a^2;

% Measurement noise
R = measurement_noise_power;

% Steps (not needed)
N = 0;

% Run simulation
sim('full_discrete_2inputs');

% Show results
close all;
figure(1);
hold on;
plot(tout,x_actual,'b-');
plot(tout(1:length(y_measure)),y_measure,'m-');
plot(tout(1:length(x_est)),x_est(:,1),'r-');
legend('True Position','Measured Position','Filtered Position','Location','Best');
title('Actual vs Kalman Filtered Position');
xlabel('Sample time [s]');
ylabel('Distance from origin [m]');
grid on;
hold off;
shg;


And here are my UPDATED results:

So with that, I have the following questions:

1. Did I inject the noise at the correct location? Is it appropriate to inject it prior to the double integration?
2. What's a good real-world noise power level to use in this application? I picked a number at random, but I imagine whatever I pick has to be considered in my Q or R matrices. I know if I bump the noise up to something like 50, the filter output and measurement value go wild, but still track each other.
3. To get the simulation to work I had to change my H matrix to 1, but [1 0] was specified in the example. Does anyone know why?
• Can you please make your title more descriptive Mar 2 '17 at 6:22

There are some problems:

1-Did I inject the noise at the correct location? Is it appropriate to inject it prior to the double integration?

The noise you injected accounts for process noise at the acceleration state, which you seem to assume as constant plus random disturbance. But then your true state generation is wrong.

2-What's a good real-world noise power level to use in this application? I picked a number at random, but I imagine whatever I pick has to be considered in my Q or R matrices. I know if I bump the noise up to something like 50, the filter output and measurement value go wild, but still track each other.

Which noise? Measurement or Process noise? Where is your measurement noise added? On the simulink model or on the script file?

3-To get the simulation to work I had to change my H matrix to 1, but [1 0] was specified in the example. Does anyone know why?

H =[1 0] is correct if your state $X = [x ~~\dot{x}]^T$ has 2 elements such as position $x$ and velocity $\dot{x}$. Becasue your measuring just the first state $x$, the position.

• Thanks for the helpful reply! So I clearly have some confusion regarding process noise and measurement noise. I was intending to inject measurement noise for my (at the moment) constant acceleration. Would my measurement noise have to be injected right before the zero-order hold? Both position and velocity are my states, so I'll update the model to include a velocity measurement (I think I can do that with a MUX). I did not intend for acceleration to be a state. Mar 2 '17 at 1:42
• Ok, well running velocity into the filter with a MUX was not as straight forward as I hoped. I have some input/output dimension errors to fix. Mar 2 '17 at 2:38
• Could you please elaborate on your statement: "But then your true state generation is wrong." I've updated the model to include what I believe is both Process and Measurement noise inputs. Mar 2 '17 at 3:30
• Now, I think you should get the intput of the true signal not before the process noise addition but after it's added to the constant. Remember that process noise is represented in the discrete filter by the Process Noise matrix $Q$. You do not have to add actual noise to the state equation. That would instead mean random input in this case. If your intention is in modeling a random accelerated object then its okay to add noise at the acceleration, but then the true signal should also include that random input. Lets change it and see if it help. Mar 2 '17 at 12:56
• I was trying to model constant acceleration from a device that gives less than great outputs (it's method of determining acceleration isn't perfect). Does this not constitute process (or plant) noise? I think what has me really confused is some of the Kalman output equations have measurement noise added to y(t) where y(t) is of the form: $$y(t) = C*x(t) + noise$$ Mar 4 '17 at 2:27