Clear Filters
Clear Filters

Kalman Filter Velocity data looking too similar to position data

39 views (last 30 days)
I'm creating a Kalman filter that intakes Accelerometer and Angular Velocity measurements from Matlab mobil IMU sensors, and filters it, then calculates the position and velocity. My question is on if I am going about calculating the position and velocity wrong, of if this is a good result but it looks strange for some reason?
Here is an abridged look at the sections of code I'm concerned with
dt = 0.1;
% Initial R and V calculation
aRaw = [Accx, Accy, Accz];
vi = aRaw .* dt;
ri = .5 * aRaw .* (dt^2);
% ~~Data is filtered here~~
% Filtered R and V calculation
af = [Accxf, Accyf, Acczf];
vf = af .* dt;
rf = .5 * af .* (dt^2);
af is the filtered acceleration data in x,y,z. I just followed basic kinematics equations, is that the wrong way to calculate it? Initial velocity and positioned are calculated the same way but from the raw data, charts are provided.
If this isn't the issue, where should I be looking to figure this out? My only other guess is maybe its an issue with the state variables. Thank you.
Tried changing sensor data, adjusting kinematic formulas, and debugging. Expected different looking results for position and velocity.

Accepted Answer

James Tursa
James Tursa on 8 Oct 2024 at 20:51
Edited: James Tursa on 8 Oct 2024 at 21:21
Your velocity vf and position rf calculations as shown represent "change" over dt, not current value. Think about it. If you had constant acceleration the entire time you would expect the overall velocity to be linear and position to be parabolic, right? But what you would have for the calculations you show are constants for both. I.e., the "changes" are constant, but the current values would not be constant. You need to be using some type of integration scheme (i.e., summing up the "changes") with these acceleration measurements, like cumtrapz( ), to get current velocity and position.
Example with constant acceleration to drive home this point:
dt = 0.1;
n = 50;
aRaw = zeros(n,3) + 0.1; % constant acceleration
t = (0:(n-1)) * dt;
vi = aRaw .* dt;
vt = cumtrapz(aRaw) * dt;
plot(t,vi(:,1)); title('vi = aRaw*dt'); xlabel('t'); ylabel('x velocity');
plot(t,vt(:,1)); title('vt = cumtrapz(aRaw)*dt'); xlabel('t'); ylabel('x velocity');
  1 Comment
Wyatt
Wyatt on 8 Oct 2024 at 21:09
cumtrapz did the trick, thank you. I had initially tried that but someone suggested formulas and it made sense at the time.

Sign in to comment.

More Answers (1)

Les Beckham
Les Beckham on 8 Oct 2024 at 19:33
It appears that you aren't differentiating the position to get the velocity, you are just multiplying it by dt. Thus the two plots look the same with only a scale factor difference. Similarly for the acceleration, you should be differentiating the velocity, not just applying a scale factor.
  1 Comment
Wyatt
Wyatt on 8 Oct 2024 at 20:49
Not sure it's any different, arent the equations of motion just a more direct form of integration?. say a point on an array is A, the integral to get velocity is Ax, and with time it just means its A*dt, which is the same thing I have (af .* dt). then the second integral of 2 is 2/2 *x^2, or 0.5 * af .* (dt^2).

Sign in to comment.

Categories

Find more on General Applications in Help Center and File Exchange

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!