filter for rotation matrix

[UX3D-mazzini]

Is this library suitable for smoothiing the values received from a rotation matrix? I can retrieve the quaternion from my rotation matrix but then I dont know how to implement the filter. I saw online that I need the Extended Kalman Filter. Any suggestions on this??

[piercus]

I've never used it for quaternion, kalman-filter can do lot of things i'm not aware of everything that can be done.
Can you give a small example data, and the expected result ?

I saw online that I need the Extended Kalman Filter

Can you share your reference ?

[UX3D-mazzini]

Thank for the fast response.
The following are the links that made me think about the extended Kalman filter.
https://stackoverflow.com/questions/29749930/kalman-filter-to-smooth-accelerometer-signals-using-rotation-matrix
https://answers.opencv.org/question/157463/filtering-noise-from-a-transformation-matrix-vs-a-quaternion/
https://campar.in.tum.de/Chair/KalmanFilter

I obtain the rotation matrix (3x3) from an object detection algorithm at each frame. the results that I obtain are very jittery, and I would like to smooth the orientation positions of those received data. I need those orientation data to be applied to a 3d model. If I convert this matrix to Euler angles and apply the rotation to my model, I see very jittery results when the angle is close to 360( i assume because of the gimbal lock?) so I'm trying to work with quaternions but I don't know if Kalman is useful for this.

[piercus]

I share you my thoughts, i might be totally off topic.

Kalman filter are working with linear observation, i feel rotation matrix are a group for the matrix multiplication, but i feel (might be wrong) they are not a group considering the matrix addition, so KF won't work.

In other words, considering 2 observations vectors (of the same phenomena) A and B (for 3x3 matrix, A will be a 9 vector), to use kalman filter, you should be sure that (A+B)/2 is the average observation of A and B.

I might be wrong but i feel that the observation model of 3x3 matrix is not fullfilling this property, and we have no garantee that (A+B)/2 is even a rotation matrix

Quaternions will face the same problem, kf will give you no easy way to garantee x² + y² + z² + w²

Anyway, quaternions will still be much better than 3x3 matrix, and you can still consider that you will renormalize the output of the kalman filter, and that (A+B)/2 makes sense when renormalized, i feel it's not as easy for strict mathematical approach, but maybe it will work as kind of first order solution ?! Or maybe it is simply working (as projective geometry might work here)

About the "extended" kalman filter, it is more for non-linear dynamic model, if you just need smoothing, without any specific dynamic model, i feel that your dynamic model will be very simple ('constant-position') and linear, and you do not need this here.

My understanding of your problem here is more about the linearity of your observation.

Once again, i might be totally wrong here, so i would be very interested if you can share your findings, and an extract of an extract of 10 quaterinons data you want to smooth.

[UX3D-mazzini]

These are the values of the quaternion elements in a static environment, when the object has a particular position, the jitter is bigger also in the quaternion value

these values are not filtered

some values:
[[-0.034069354445983836,-0.4203138371138492,0.06025153943775143,0.886648292935227],[-0.05997317513158425,-0.38543887834672014,0.07656194074978731,0.8988347709373589],[-0.07633086356161999,-0.36770522569086,0.0848408527112643,0.9038076045248841],[-0.01251942459757149,-0.4102471765716491,0.0693680296047438,0.8873314820059705],[-0.04531256818164259,-0.4211866793202373,0.08146521058293887,0.8833874880786647],[-0.010680208635551315,-0.4147517158396646,0.05710879968221693,0.8876962063725066],[0.0002540045634708749,-0.4129695461478918,0.024177595215241397,0.8910094485472287],[-0.00268198858160836,-0.3783039531186383,0.002616143812900204,0.9064547907525459],[0.006252282518922982,-0.34636637080475347,0.057102840627490464,0.9114093013472336],[0.009121871823081915,-0.35399665320953766,0.03337024160955788,0.9077737470027399],[-0.020811107540520023,-0.39721581849492393,0.08461235998280144,0.8963112050935474],[-0.011083862268189205,-0.4112958533972543,0.08952763089572584,0.8877968389116029],[0.005621095656672467,-0.41497768609622315,0.04340637421836848,0.8904005152564815],[-0.008616670647913712,-0.40690463123735543,0.05517907050747135,0.8934504922453872],[0.003963298281618984,-0.426140856093328,0.043403155342372574,0.8860447838831056],[-0.0172791487288109,-0.4005158782632011,0.04393219760180177,0.8957025696810547],[-0.01439456603790589,-0.39335314648702446,0.055788273324766435,0.8977712067914658],[-0.00983489761845114,-0.41831888373948506,0.06424619241961124,0.8871049124144798],[-0.0077388616928696025,-0.41573431375119,0.06820787548408944,0.8879977677259224],[-0.007829160283509797,-0.4152983679284729,0.07102977860060557,0.887974341658681]]

[piercus]

@UX3D-mazzini see https://observablehq.com/@piercus/kalman-filter-issue-37

[UX3D-mazzini]

Thank you very much, I'll check this now.
Just want to say that reading from the links I don't know if smoothing the matrix is good, because some values must be equal to one to be coerent. So I'll try to work with the quaternions.
Thank you for the help

[piercus]

Yes, i feel that you will need to renormalize quaternions anyway in the output, it might create some strange behavior, please keep me posted of the progress

If you can create a better observablehq (or a fiddle) about this, explaining your steps, please share it, i could use as an example in the README.md for the community

[UX3D-mazzini]

Hi, I'm implementing your solution. Can I ask you the difference between the state projection matrix and the observed projection matrix? why I cannot define both of them?

[UX3D-mazzini]

this comment to show you the results!! seems nice!

var {KalmanFilter} = kalmanFilter;
const kFilter = new KalmanFilter({
dynamic: {
name: 'constant-position',
dimension: 4,
covariance: [
[1, 0, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]
],
transition:
[
[1, 0, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]
],
init:{
mean: [[0], [0], [0], [0]],
covariance:[
[1000000, 0, 0, 0],
[0, 1000000, 0, 0],
[0, 0, 1000000, 0],
[0, 0, 0, 1000000]
],
index: -1
}
},
observation: {
name: 'sensor',
sensorDimension: 4,
sensorCovariance: 10,
dimension: 4,
observedProjection:[
[1, 0, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]
],
covariance: [//10
[0.0001, 0, 0, 0],
[0, 0.0001, 0, 0],
[0, 0, 0.0001, 0],
[0, 0, 0, 0.0001]
],
//stateProjection: [
// [1, 0, 0, 0],
// [0, 1, 0, 0],
// [0, 0, 1, 0],
// [0, 0, 0, 1]
// ]
}
});

Some points before filtering:

[[0.08894225713878627,-0.5302104332063636,-0.30453416750449797,0.6754148494651938],[0.12879511140275168,-0.5232952149453235,-0.284579420977486,0.6905897526894842],[0.15174833319192843,-0.49756866414062917,-0.2795917631958591,0.7033092942708244],[0.1617475768810654,-0.5035413904274849,-0.27670332544092946,0.6914987212645157],[0.18346162827436677,-0.483921057269954,-0.2674546121457084,0.703951601415562],[0.1777013477039221,-0.48934407900438026,-0.26964251463218025,0.6988927856250534],[0.1818462135858313,-0.4972201683865893,-0.2657518939388442,0.6955832238095053],[0.1892058358620858,-0.4943509285171406,-0.2551328838406259,0.7044902957217181],[0.1847703978873224,-0.4883924292567647,-0.26132254568963403,0.6978236986723692],[0.19653686836452416,-0.47444767785029635,-0.26089390518982203,0.707588031235516],[0.19105649542885123,-0.474987895150619,-0.25907950674801755,0.7139178152435416],[0.1916911141029046,-0.47231766758877664,-0.26085417602383737,0.7110440516927258],[0.1927464250310751,-0.48128887074007237,-0.2543402625586021,0.7113310715516392],[0.1849769627884189,-0.4719236775980047,-0.26285463011483895,0.716068881047498],[0.18517611121423672,-0.47182380031340293,-0.266677978030859,0.7094192356783041],[0.17990255314939388,-0.4795653513734308,-0.25973510129600214,0.7190263711800358],[0.18511817503580832,-0.46950235431083487,-0.2670309077818661,0.7143761921247538],[0.18397237794878177,-0.46644914080036903,-0.26205332840662765,0.7234727226972546],[0.18480371204952042,-0.4706100800440734,-0.2682825796750029,0.7103857697013971],[0.18374166891893326,-0.47002337920314075,-0.2635744336014278,0.7243721248963048],[0.18581566283123377,-0.4690208721534061,-0.2699238828264765,0.7133808840933764]]

some points after filtering

[[0.08894136772599842,-0.5302051311603541,-0.30453112219632134,0.675408095390994],[0.10981704785256907,-0.5265856669068485,-0.29408027632338357,0.6833603576458286],[0.12592551143892,-0.5154383975431321,-0.28851432039700936,0.6910240082258057],[0.13761137574728705,-0.5115573591632078,-0.2846613408800558,0.6911788689823619],[0.15131352952808372,-0.5032983673888389,-0.2795191828724585,0.6949959459744093],[0.15883735365922777,-0.4993196522491361,-0.27670309878599353,0.6961070320067346],[0.1652348164073108,-0.4987359045170151,-0.2736581880307562,0.6959613905694968],[0.17181086947701876,-0.4975329588125584,-0.2685760769542282,0.6983011547590561],[0.17534071532139983,-0.49504331109958233,-0.2666003995699293,0.6981711078676796],[0.18109199371739473,-0.4894549753144302,-0.2650520224762933,0.7007262577485087],[0.18379022221097663,-0.4855375202276346,-0.2634347602605749,0.7042983215946014],[0.1859273384874644,-0.4819616755465882,-0.2627367367145479,0.7061229775684698],[0.18777077106312187,-0.4817797933708073,-0.2604668822581267,0.7075309036952698],[0.1870157414994299,-0.47911616793200446,-0.261112173575639,0.7098383008683096],[0.1865186610426973,-0.4771457212310055,-0.2626160913154987,0.7097250666398207],[0.18473110041851445,-0.47779946443432647,-0.2618376964645428,0.7122381220521199],[0.18483567657722805,-0.47555782970455635,-0.26324074917502654,0.7128157656000739],[0.18460244450085816,-0.4730969912922975,-0.26291995101525556,0.7156948900995923],[0.18465681899765846,-0.4724251266593353,-0.26436872026215436,0.7142605766594835],[0.18440958353720588,-0.4717762744633652,-0.2641541370666094,0.7169922961253239],[0.18478944621489166,-0.4710318822345569,-0.2657128763804007,0.7160166465652252]]

[piercus]

Can I ask you the difference between the state projection matrix and the observed projection matrix? why I cannot define both of them?

Let's define

D = dynamic.dimension
O = observation.dimension

In your use case, O === D there are the same, it's an alias.

The internal state is often made of observed and non observed dimension.

For example when using constant-speed dynamic, 'position' is observed and speed is not-observed.

D1 = observed dimension
D2 = not observed dimension 
D1 + D2 = D

Then the stateProjection matrix is a OxD matrix and can be viewed as a [OxD1 OxD2] matrix.

OxD1 is the observedProjection matrix
OxD2 is a zero matrix

In the lib you can either use stateProjection as a OxD matrix or define observedProjection as a OxD1 matrix, but not both

this comment to show you the results!! seems nice!

Great ! Thanks for sharing I'm closing the issue

[UX3D-mazzini]

Hello, sorry to reopen the issue but I have some questions on the theoretical aspect of this.
"About the "extended" kalman filter, it is more for non-linear dynamic model, if you just need smoothing, without any specific dynamic model, i feel that your dynamic model will be very simple ('constant-position') and linear, and you do not need this here."
From this sentence I understand that in this case I use a linear constant position filter. But what constant position means?
What is the difference from using the dynamic keyword in the definition of the filter and not using it?
Is this the same model I would use to smooth the x,y,z, coordinates of the position in the space of an object(not interested in speed or acceleration)?

in the following example I have questions about the matrices:

const kFilter = new KalmanFilter({
dynamic: {
name: 'constant-position',
dimension: 4,
covariance: [ // is this variance in movement caused by user, is this matrix based on the stochastic nature of the model?the uncertainty of the estimation?
[1, 0, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]
],
transition:
[ // is this based on the model and how it develops from step to step? it describes how the value develops according to the model?
[1, 0, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]
],
init:{
mean: [[0], [0], [0], [0]],
covariance:[ //is this the initial covariance that we dont know at the beginning? prior knowledge of state? the covariance matrix of the estimation, meaning the uncertainty of the estimation?
[1000000, 0, 0, 0],
[0, 1000000, 0, 0],
[0, 0, 1000000, 0],
[0, 0, 0, 1000000]
],
index: -1
}
},
observation: {
name: 'sensor',
sensorDimension: 4,
sensorCovariance: 10,
dimension: 4,
observedProjection:[ //describes the relation between the model and the observation?
[1, 0, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]
],
covariance: [ //is this the covariance matrix of the noise of the sensor? variance of the noise of the sensor? it would make sense to observe the data in a static setting calculate the variance and substitute it here for each coordinate?
[0.0001, 0, 0, 0],
[0, 0.0001, 0, 0],
[0, 0, 0.0001, 0],
[0, 0, 0, 0.0001]
],

}
});

I hope you can help me clarify this, thank you

[UX3D-mazzini]

https://www.ncbi.nlm.nih.gov/pmc/articles/PMC6339217/

Although there are other mathematical tools used for estimation [1], the Kalman Filter [2] has become the algorithm par excellence in this area. Because of its simplicity, the rigor and elegance in its mathematical derivation, and its recursive nature it is very attractive for many practical applications. Its non-linear versions have been widely used in orientation estimation: the Extended Kalman Filter (EKF), and the Unscented Kalman Filter (UKF) [3]. However, there are problems arising from the used parametrization to represent the orientation.
....
This fact implies a problem in applying the ordinary Kalman Filter, so different approaches have emerged. Since a quaternion is of dimension 4, one tends to think at first on a 4×4 covariance matrix, and in the direct application of the Kalman Filter [13]. Given that all predictions are contained in the surface defined by the unit constraint, the covariance matrix shrinks in the orthogonal direction to this surface, which leads to a singular covariance matrix after several updates.

Also, here is written this, so Im wondering if this is the right way to proceed. Ill investigate more this

[piercus]

From this sentence I understand that in this case I use a linear constant position filter. But what constant position means?

Let's consider a 1 dimension problem, with 1 variable x.

In a kalman-filter, you will always "predict" the next point considering the previous point.

constant-position

"constant-position" model means that you do

$x_t \sim x_{t-1}$

constant-speed

in a constant-speed model you will have
$x_t \sim x_{t-1} + speed_{t-1}$
$speed_t \sim speed_{t-1}$

And the $state$ of your kalman filter will be a 2 dimension vector $state_t = [x_t, speed_t]$

constant-acceleration

constant-acceleration will be a 3d state.

$x_t \sim x_{t-1} + speed_{t-1}$
$speed_t \sim speed_{t-1} + acc_{t-1}$
$acc_t \sim acc_{t-1}$

and state will be $state_t = [x_t, speed_t, acc_t]$

questions in comments

To be more precise about matrix meaning, i made this correspondance table with wikipedia article

Wikipedia article	kalman-filter js lib
$F_k$, the state-transition model	dynamic.transition
$H_k$, the observation model	observation.stateProjection related to observation.observedProjection as explained in here
$Q_k$, the covariance of the process noise	dynamic.covariance
$R_k$, the covariance of the observation noise	observation.covariance
$\mathbf{P}_{0\mid 0}$	dynamic.init.covariance
$\mathbf{x}_{0\mid 0}$	dynamic.init.mean