Usbrth

What was the last x86 CPU that did not have the x87 floating-point unit built in?

What's the point in a preamp?

Presidential Pardon

Variable with quotation marks "$()"

Drawing vertical/oblique lines in Metrical tree (tikz-qtree, tipa)

Are spiders unable to hurt humans, especially very small spiders?

Can each chord in a progression create its own key?

Simulating Exploding Dice

Do warforged have souls?

Identify 80s or 90s comics with ripped creatures (not dwarves)

ELI5: Why do they say that Israel would have been the fourth country to land a spacecraft on the Moon and why do they call it low cost?

What happens to a Warlock's expended Spell Slots when they gain a Level?

What information about me do stores get via my credit card?

What force causes entropy to increase?

Didn't get enough time to take a Coding Test - what to do now?

How to politely respond to generic emails requesting a PhD/job in my lab? Without wasting too much time

Store Dynamic-accessible hidden metadata in a cell

Python - Fishing Simulator

Was credit for the black hole image misappropriated?

How do I design a circuit to convert a 100 mV and 50 Hz sine wave to a square wave?

Intergalactic human space ship encounters another ship, character gets shunted off beyond known universe, reality starts collapsing

Is this wall load bearing? Blueprints and photos attached

How to make Illustrator type tool selection automatically adapt with text length

Would an alien lifeform be able to achieve space travel if lacking in vision?

Extended Kalman Filter for Orientation without Control Input

0

$begingroup$

I'd like to implement an Extended Kalman Filter to estimate the state$$s=[w,x,y,z,av_x,av_y,av_z]$$with $[w,x,y,z]^T$ respresenting the current orientation as a quaternion and $[av_x,av_y,av_z]^T$ representing the estimated angular velocity.

Moreover I do not have a control input (besides a timestamp). My measurements are quaternions. My prediction function would estimate the new state like this. The angular velocities stay constant. The quaternion is predicted with the current estimated angular velocity and the old quaternion. In Pseudo code:

$$
q_t = q_t-1 times (textAngular velocity × ΔtextTime)
$$
In particular I used this approach: https://stackoverflow.com/a/24201879/654005

Moreover I've implemented the Jacobian (Following https://en.wikipedia.org/wiki/Extended_Kalman_filter notation)
$$
F = frac dfx
$$

The H Matrix (Jacobian of Measurement) has just ones since h(x) just takes the quaternion part of x to get the measurement z.

I initialized the State with 0. Unfortunatelly my Jacobian returns "NaN" for most of the values if Angular Velocity is 0. Thus I modified the Jacobian by checking the Angular Velocity for 0. If it is 0, I return an Identity Matrix. But now the AV stays at 0 forever. If I initialize AV with small values it is updated.

My noise matrices are ordinary diagonal matrices representing the process and measurement noise.

Is this the correct approach to estimate orientation and orientation derivative (= angular velocity) with an extended kalman filter?

Thanks for your support.

estimation kalman-filter

share|cite|improve this question

edited Apr 8 at 21:43

Henno Brandsma

116k349127

asked Apr 8 at 7:01

SoccertrashSoccertrash

1013

$endgroup$

• 
  
  
  

  
  

  
  
  
  
  
  

  
  $begingroup$
  A better approach might be to use a multiplicative extended Kalman filter.
  $endgroup$
  – Kwin van der Veen
  Apr 8 at 13:35
  

  
  
  
  


• 
  
  
  

  
  

  
  
  
  
  
  

  
  $begingroup$
  Thank you. Can you please be more specific, why this would be a better approach? Do you have an implementation of this approach as well?
  $endgroup$
  – Soccertrash
  Apr 8 at 14:25
  

  
  
  
  

add a comment | 

0

$begingroup$

I'd like to implement an Extended Kalman Filter to estimate the state$$s=[w,x,y,z,av_x,av_y,av_z]$$with $[w,x,y,z]^T$ respresenting the current orientation as a quaternion and $[av_x,av_y,av_z]^T$ representing the estimated angular velocity.

Moreover I do not have a control input (besides a timestamp). My measurements are quaternions. My prediction function would estimate the new state like this. The angular velocities stay constant. The quaternion is predicted with the current estimated angular velocity and the old quaternion. In Pseudo code:

$$
q_t = q_t-1 times (textAngular velocity × ΔtextTime)
$$
In particular I used this approach: https://stackoverflow.com/a/24201879/654005

Moreover I've implemented the Jacobian (Following https://en.wikipedia.org/wiki/Extended_Kalman_filter notation)
$$
F = frac dfx
$$

The H Matrix (Jacobian of Measurement) has just ones since h(x) just takes the quaternion part of x to get the measurement z.

I initialized the State with 0. Unfortunatelly my Jacobian returns "NaN" for most of the values if Angular Velocity is 0. Thus I modified the Jacobian by checking the Angular Velocity for 0. If it is 0, I return an Identity Matrix. But now the AV stays at 0 forever. If I initialize AV with small values it is updated.

My noise matrices are ordinary diagonal matrices representing the process and measurement noise.

Is this the correct approach to estimate orientation and orientation derivative (= angular velocity) with an extended kalman filter?

Thanks for your support.

estimation kalman-filter

share|cite|improve this question

edited Apr 8 at 21:43

Henno Brandsma

116k349127

asked Apr 8 at 7:01

SoccertrashSoccertrash

1013

$endgroup$

• 
  
  
  

  
  

  
  
  
  
  
  

  
  $begingroup$
  A better approach might be to use a multiplicative extended Kalman filter.
  $endgroup$
  – Kwin van der Veen
  Apr 8 at 13:35
  

  
  
  
  


• 
  
  
  

  
  

  
  
  
  
  
  

  
  $begingroup$
  Thank you. Can you please be more specific, why this would be a better approach? Do you have an implementation of this approach as well?
  $endgroup$
  – Soccertrash
  Apr 8 at 14:25
  

  
  
  
  

add a comment | 

$begingroup$

I'd like to implement an Extended Kalman Filter to estimate the state$$s=[w,x,y,z,av_x,av_y,av_z]$$with $[w,x,y,z]^T$ respresenting the current orientation as a quaternion and $[av_x,av_y,av_z]^T$ representing the estimated angular velocity.

Moreover I do not have a control input (besides a timestamp). My measurements are quaternions. My prediction function would estimate the new state like this. The angular velocities stay constant. The quaternion is predicted with the current estimated angular velocity and the old quaternion. In Pseudo code:

$$
q_t = q_t-1 times (textAngular velocity × ΔtextTime)
$$
In particular I used this approach: https://stackoverflow.com/a/24201879/654005

Moreover I've implemented the Jacobian (Following https://en.wikipedia.org/wiki/Extended_Kalman_filter notation)
$$
F = frac dfx
$$

The H Matrix (Jacobian of Measurement) has just ones since h(x) just takes the quaternion part of x to get the measurement z.

I initialized the State with 0. Unfortunatelly my Jacobian returns "NaN" for most of the values if Angular Velocity is 0. Thus I modified the Jacobian by checking the Angular Velocity for 0. If it is 0, I return an Identity Matrix. But now the AV stays at 0 forever. If I initialize AV with small values it is updated.

My noise matrices are ordinary diagonal matrices representing the process and measurement noise.

Is this the correct approach to estimate orientation and orientation derivative (= angular velocity) with an extended kalman filter?

Thanks for your support.

estimation kalman-filter

share|cite|improve this question

edited Apr 8 at 21:43

Henno Brandsma

116k349127

asked Apr 8 at 7:01

SoccertrashSoccertrash

1013

$endgroup$

share|cite|improve this question

edited Apr 8 at 21:43

Henno Brandsma

116k349127

asked Apr 8 at 7:01

SoccertrashSoccertrash

1013

share|cite|improve this question

edited Apr 8 at 21:43

Henno Brandsma

116k349127

asked Apr 8 at 7:01

SoccertrashSoccertrash

1013

edited Apr 8 at 21:43

Henno Brandsma

116k349127

edited Apr 8 at 21:43

Henno Brandsma

116k349127

asked Apr 8 at 7:01

SoccertrashSoccertrash

1013

asked Apr 8 at 7:01

SoccertrashSoccertrash

1013