Kalman Filter as Constraint Solver

Please don't post Bullet support questions here, use the above forums instead.
Post Reply
Antonio Martini
Posts: 126
Joined: Wed Jul 27, 2005 10:28 am
Location: SCEE London

Kalman Filter as Constraint Solver

Post by Antonio Martini »

In my spare time i have been investigating the solution of constraints at positional level by using the Extended Kalman Filter. Some results are exposed below. If you are not familiar with the basics of Kalman Filtering you are strongly adviced to read the references at the bottom of this post first. The following analysis may contain minor(or major) errors, however, after spending enough time on it(maybe after filling the missing equations and reading the related papers) it should be enough in order to give you an idea of how EKF is applied.

if we replace the term 'time' with 'iteration' we can set up the following process model for the Extended Kalman Filter(EKF)(see refs 1 and 2 at the bottom of this post):

x' = x + p

x' = value of x(positions) at the next iteration. x could be a block vector of positions < p1, p2, p3 etc..>
x = current value of x, where x is the set of state variables we want to restrict.
p = is the process noise variable having zero mean and covariance Q.

we can consider each contraint imposed on x as a nonlinear measurement function of the state x:

C(x) = vector of nonlinear constraints we want to impose on the state x. [ C_0(x) C_1(x) ... ]_t (_t = transpose, C(x) is a column vector)

typical constraints are scalar quantities that must be conserved: length, area, volume, etc...

given that constraints are on the form C(x) = 0 we use the following measurement equation:

0 = C.i(x) + v.i ( i = constraint index in C(x) )

v.i = is the measurement noise variable having zero mean and covariance R. This noise can for example be due to floating point inaccuracies.

the use of an additive error model will make the matrices W and V in equations 4.18-4.19 to disappear(see ref (1) below). This less general assumption is also used in (2)(equations 28 and 29).

Having defined both the process model and the measurement model for EKF we get a set of predict-correct equations similar to eq.4.27-4.31. They are just eq.4.27-4.31 without W, V, u and z(=0 for us).

eq. 4.27 in our case will become just x' = x, so A = identity. We have a linear prediction with and nonlinear measurement model.
if we replace the H with J (constraints jacobian) and set the initial
P=W(state covariance matrix = (sub)system inverse mass matrix) we have the following:

prediction:

a) x' = x; // state prediction
b) W' = W + Q; // state covariance prediction (this is not the same W we dropped from eq. 4.27-4.31)

correction:

x' = x - W*J_t*[J*W*J_t + R]_i*C(x) ( _t = transpose , _i = inverse )
or

x' = x - K*C(x)

c) K = W*J_t*[J*W*J_t + R]_i (W*J_t is repeated twice (W*J_t)*[J*(W*J_t) + R]_i)

if we add the stiffness by replacing the constraints in C(x) with s_i*C_i(x) we get:

x' = x - K*S*C(x) S: diagonal matrix diag( s_0 s_1 s_2 etc...) containing the stiffness values.

and so we get the following correction formulas:

d) x' = x - K*S*C(x)
e) W' = (I - K*J)*W

where Q and R are respectively the process noise and measurement noise _diagonal_ covariance matrices.

R has the same meaning of CFM(Contraint Force Mixing) used typically in constrained rigid body dynamics.
S is the equivalment of the ERP. As we know the two terms can be used to model a richer set of behaviours(eg. material properties) than
ERP alone.

summarizing...

prediction:

a) x' = x;
b) W' = W + Q; // the state covariance prediction W is intialised to the inverse system mass matrix before entering the the loop

correction:

c) K = W*J_t*[J*W*J_t + R]_i // for nonlinear constraints J should be recomputed at every iteration
d) x' = x - K*S*C(x)
e) W' = (I - K*J)*W

as an example we can try to derive a edge length constraint using the same notation of the position based dynamics paper(see Muller....), we write the state, mass etc.. in matrix block form:

x = [ p1_t p2_t ]_t (this is simply a column vector with p1 and p2)

J = [ -n1_t n_t ]_t

W = [ (1/m1)*I3x3 0 ] (I3x3 = identity 3x3 matrix)
[ 0 (1/m1*I3x3) ]

as we have one constraint s = a scalar stiffness value.

Q = 0;
R = in this special case is a scalar, specifically the measurement noise variance, we set it to a small scalar in order to account for floating point inaccuracies.

by plugging the above defined x, J, W, Q and R in eq. (c-d) and after simplifying(n_t*n = 1 etc.)we get the equations given in the PBD paper and Jacobsen as a special case (Q=0, R=0, number of constraints =1).

The advantages of the EKF approach is that the mass weighting is not added ad-hoc afterwards(for rigid bodies the weighting maybe non trivial), we also have a better numerically conditioned system due to R, which in turn should allow for wider range of materials(see ERP, CMF discussion above). Q and R dont need to be the same at each iteration, so for example Q for the current iteration number could be determined offline in order to obtain some pseudo optimal convergence for a specific system. Further C(x) can be a vector of n constraints. This can be used in order to build a block sparse iterative solver.
Multiple constraints(=measurements) can be handled by either increasing C(x) and x, the SCAAT method(Single Constraint At a time) described in 5.4 of (1)(same as relaxation) or by a combination of both. This way we can trade off accuracy vs cpu time.
Further the state x doesnt need to be just particles positions, it would be interesting to see how it behaves when applied to rigid bodies.
If we want higher accuracy we could also use the the Unscented Kalman Filter(UKF) that doesn't require any explicit jacobian computation.

for the constraints with _equal_masses_ i tested EKF with (C(x) having a single constraint and SAAT in order to handle multiple constraints), eq. (e) (Q != 0 and using same variance for all nodes), didnt seem to have a significat effect on the result and so i set W to be the constant system inverse mass matrix at every iteration without using (e) and Q = 0 for this case.

for constraints involving different masses instead, both Q(using the same variance for all nodes) and equation (e) can be used in order to improve the solver convergence. for Q = infinite(large number in practice), masses are weighted with equal factor, and for Q = 0 we get a weigthing reflecting the relative mass as usual.
So the repeated adding of Q to W in eq(b) is effectively cancelling out the relative mass effect weight to some degree at each iteration. So at each iteration the masses will gradually be seen as more "similar" by making it easier for the solver to converge.

The next question that arises is? how can we better estimate the uncertainity at the nodes(expressed by the node variances in the Q matrix) in order to further improve the solver convergence? for example node variances could be estimated at each iteration from the error present in the constraints connected to them by a biased/unbiased variance estimation. This operation would not be theoretically correct as constraints are not independent, however it may still provide a reasonable rough estimate.

Further investigation is required. Given that i have run out of spare time, if you would like to collaborate in order to expand/test/improve/discuss the above approach you are very welcome to message/email me.

thanks,
Antonio

edited: added a few considerations on the role of Q and possible ideas. Im changing it here otherwise multiple posts would make it too messy and inconsistent.

(1) An Introduction to the Kalman Filter
Greg Welch, Gary Bishop, Siggraph 2001
http://www.cs.unc.edu/~tracker/media/pd ... ack_08.pdf

(2) Kalman Filters: A Tutorial
J. D. Schutter, J. D. Geeter [...]
http://citeseer.ist.psu.edu/cache/paper ... torial.pdf
Last edited by Antonio Martini on Fri Oct 26, 2007 6:30 pm, edited 18 times in total.
Erin Catto
Posts: 316
Joined: Fri Jul 01, 2005 5:29 am
Location: Irvine
Contact:

Re: Kalman Filter as Constraint Solver

Post by Erin Catto »

There is a paper on a variation of Featherstone's method using Kalman Filtering that you might find useful.

Kalman Filtering, Smoothing, and Recursive Robot Arm Forward and Inverse Dynamics, Guillermo Rodriguez, IEEE Journal of Robotics and Automation, vol RA-3, no 6, December 1987.

You can find a scan here: http://ntrs.nasa.gov/archive/nasa/casi. ... 008018.pdf

I like to view position correction as a optimization problem. In that context the mass weighting is less adhoc.

min 0.5 d_t * M * d
such that C(x + d) >= 0

You can solve this using Nonlinear Gauss Seidel iterations where d is a virtual velocity. Therefore, the solver is trying to minimize the virtual momentum gain as it projects the positions onto the constraint manifold.
Antonio Martini
Posts: 126
Joined: Wed Jul 27, 2005 10:28 am
Location: SCEE London

Re: Kalman Filter as Constraint Solver

Post by Antonio Martini »

Erin Catto wrote:There is a paper on a variation of Featherstone's method using Kalman Filtering that you might find useful.

Kalman Filtering, Smoothing, and Recursive Robot Arm Forward and Inverse Dynamics, Guillermo Rodriguez, IEEE Journal of Robotics and Automation, vol RA-3, no 6, December 1987.

You can find a scan here: http://ntrs.nasa.gov/archive/nasa/casi. ... 008018.pdf

I like to view position correction as a optimization problem. In that context the mass weighting is less adhoc.

min 0.5 dT * M * d
such that C(x + d) >= 0

You can solve this using Nonlinear Gauss Seidel iterations where d is a virtual velocity. Therefore, the solver is trying to minimize the virtual momentum gain as it projects the positions onto the constraint manifold.
Thanks a lot for posting the link, i promised myself yesterday to have a look again at the Rodrigues's work today but i forgot about it :) I "read" the paper you mention or a related one many years ago discarding it as an option even before understanding it and moved directly to Featherstone. Time to read it properly i would say:)

Regarding EKF and the constrained minimization approach you mention in the end they are very similar, see also section 3.1 of:

http://academic.csuohio.edu/simond/kalm ... lmaneq.pdf

what i liked about the Kalman Filter is the possibility of feeding the solver with some probability distributions that are supposed to compensate for unmodelled factors(discarded constraint-constraint interactions etc...) so it seemed conceptually slighly more general(see also the Unscented Kalman Filter etc..) as a method of modeling as we can account for uncertainity. So it seems that there is more scope for improvement, at least at conceptual level.

cheers,
Antonio
Last edited by Antonio Martini on Wed Jan 09, 2008 1:06 am, edited 5 times in total.
Dirk Gregorius
Posts: 861
Joined: Sun Jul 03, 2005 4:06 pm
Location: Kirkland, WA

Re: Kalman Filter as Constraint Solver

Post by Dirk Gregorius »

your dT which i thought was the timestep is actually d_t(transpose):)
ERIN: I edited this for you quickly, so it fits the notation of the post.
raigan2
Posts: 197
Joined: Sat Aug 19, 2006 11:52 pm

Re: Kalman Filter as Constraint Solver

Post by raigan2 »

AntonioMartini wrote:K is the equivalment of the ERP
Do you mean S? I'm confused..
Antonio Martini
Posts: 126
Joined: Wed Jul 27, 2005 10:28 am
Location: SCEE London

Re: Kalman Filter as Constraint Solver

Post by Antonio Martini »

raigan2 wrote:
AntonioMartini wrote:K is the equivalment of the ERP
Do you mean S? I'm confused..
yes S sorry, thanks for pointing that out, i have edited the post.

cheers,
Antonio
raigan2
Posts: 197
Joined: Sat Aug 19, 2006 11:52 pm

Re: Kalman Filter as Constraint Solver

Post by raigan2 »

oh good.. I was worried that I just totally misunderstood everything! ;)
Post Reply