Kalman Filter Covariance Propagation in RIC

aerow610 · November 19, 2019, 11:44pm

Hi,

I am currently running the Kalman Filter on satellite in low earth orbit and using the Python Wrapper. For this satellite, it would greatly simplify things for me to have the covariance and process noise matrices represented in a local RIC (or TNW) frame. From what I understand, since my initial state and filter output is in the J2000 frame (and kalman propagator is inheriting these in cartesian), both of the aforementioned matrices are represented in this frame.

My current idea for this is to have my getProcessNoiseMatrix function (instance of CovarianceMatrixProvider) take the input J2000 covariance, transform it to the RIC frame, perform the evolution similar to here on p. 5 CovarianceReference, then transform it back to J2000. This method of 2 transforms seems a bit clunky, and also makes any intermediate covariance propagation between measurements difficult as each step will have to do the same. Plus I would guess that this will significantly increase the filter processing time.

Thanks!

MaximeJ · November 20, 2019, 10:53am

Hi @aerow610,

Yes, they have to when they are given to the filter. So you need to transform from RIC to J2000 or backwards at every step of the Kalman.
The transformation has to be done at some point since it is not possible to propagate the orbit and covariance in RIC frame.

The interface CovarianceMatrixProvider was made for the kind of purpose you’re looking for. You need to implement this interface with your own code and perform the transformation from RIC to inertial in there.

Which is what you propose in your second point. Regarding this, I think your idea is a good one.
There is actually something close to what you’re looking for, it is the class UnivariateProcessNoise. Here you can define your process noise matrix with time-dependent univariate functions (like polynomials in your case) in RIC (or TNW… what we call LOF for Local Orbital Frame in Orekit ) frame.

I don’t have much time right now so I cannot read the whole paper you put in reference.
But I think you will encounter two problems:

The sigmas used to compute the QRIC matrix seems to come from the current covariance matrix. In class UnivariateProcessNoise the coefficients of the univariate functions are defined at initialization. It will require a change of interfaces to add a covariance matrix in input.
The UnivariateProcessNoise class assumes the process noise in RIC is a diagonal matrix while in your paper it is obviously not the case.

Tell me if my assumptions regarding the issues you may face are right.
It could be interesting to improve our models so if you need the UnivariateProcessNoise to evolve please open a bug on the Gitlab forge of the Java version of Orekit.

MaximeJ · November 22, 2019, 8:45am

Hi @aerow610,

Edit from my previous post.
I’ve read the paper you mention in your first post.
I found it very interesting and if you have time I will be happy to have a follow up from you on whether you managed or not to reproduce the process noise matrix presented in the paper.

From what I’ve understood the “problem 1” I gave in my previous post is not an issue since the sigmas used in QRIC are computed by a post-processing, apart from the Kalman filter.

You’ll still get the “problem 2” of not being able to define the cross-coupling terms in the process noise matrix QRIC.
If you want this to be corrected for version 10.1 of Orekit, please open an issue on the tracker of the forge.

Cheers,
Maxime

aerow610 · December 9, 2019, 11:18pm

Hi @MaximeJ,

Thank you for all the info. I have just gotten back into this, so haven’t had time to try out the UnivariateProcessNoise class yet. I imagine that it will be fine using only the diagonal elements since I am working with less accurate orbit determination measurements and don’t need to be too precise at this point. Will let you know if I get it working.

I did however attempt to do the transform for each step, though no longer seem to have a converging filter as a result. Below is a code snippet of my implementation:

class filt_covar_provider(PythonCovarianceMatrixProvider):
    def getInitialCovarianceMatrix(self, initial):
        i_cov = JArray_double(len(init_cov))
        # Initial diagonal covariance into Java
        for x,i in enumerate(init_cov):
            i_cov[x] = i
        return MatrixUtils.createRealDiagonalMatrix(i_cov)
    def getProcessNoiseMatrix(self, prev, curr):
        t_meas = abs(curr.getDate().durationFrom(prev.getDate()))
        if t_meas > 0.0:
            meas_delta_t = t_meas
        else:
            t_meas = meas_delta_t
        return xform_vnc_to_j2k(curr, get_proc_noise_mat(t_meas))

vnc_acc_sig = [(2.4e-11)**2, (1.4e-9)**2, (5e-9)**2] # final values from AURA in paper

# Custom Process Noise Matrix
def get_proc_noise_mat(t):
    Q = Array2DRowRealMatrix(6, 6)
    for i in range(3):
        Q.setEntry(i, i, (vnc_acc_sig[i] * t**4 / 3))
        Q.setEntry(i, i+3, (vnc_acc_sig[i] * t**3 / 2))
    for i in range(3,6):
        Q.setEntry(i, i, (vnc_acc_sig[i-3] * t**2))
        Q.setEntry(i, i-3, (vnc_acc_sig[i-3] * t**3 / 2))
    return RealMatrix.cast_(Q)

# Transform the covariance to J2000
def xform_vnc_to_j2k(curr_state, cov_mat_vnc):
    # Get dummy propagator to build VNC frame
    propagator_num.resetInitialState(curr_state)
    vnc = LocalOrbitalFrame(j2k_frame, LOFType.VNC, propagator_num, 'VNC')
    sc_state = propagator_num.getInitialState()
    
    # Get frozen orbit and Jacobian
    vnc_to_j2k_frozen = vnc.getTransformTo(j2k_frame, sc_state.date).freeze()
    jacobianDoubleArray = JArray_double2D(6, 6)
    vnc_to_j2k_frozen.getJacobian(CartesianDerivativesFilter.USE_PV, jacobianDoubleArray)
    jacobian = Array2DRowRealMatrix(jacobianDoubleArray)

    cov_mat_j2k_java = jacobian.multiply(cov_mat_vnc.multiply(cov_mat_vnc.transpose()))
    return cov_mat_j2k_java

To start off, I just took the sigmas from Aura listed in the paper (can calibrate later once I get it working).

When I run the filter, it diverges fairly quickly, so I am afraid that my implementation of the transform was not done properly.

MaximeJ · December 10, 2019, 12:55pm

Hi @aerow610,

I think there’s an error in your code, right before the last line it should be:

cov_mat_j2k_java = jacobian.multiply(cov_mat_vnc.multiply(jacobian.transpose()))

And I’ll add two remarks:

Not sure if it works in Python but most of the code of xform_vnc_to_j2k function where you get the Jacobian could be replaced with the line (assuming your current state is already in J2000):

LOFType.VNC.transformFromInertial(curr_state.getDate(), curr_state.getPVCoordinates()).getInverse().getJacobian(CartesianDerivativesFilter.USE_PV, jacobianDoubleArray );

I think the sigmas you input may not be the good ones. You did the transform from RIC to VNC by assuming VNC = IRC; while I think it should be VNC = ICR.
Nevertheless this does not explain the divergence.

Maxime

aerow610 · February 7, 2020, 11:50pm

Hi @MaximeJ,

I realized I never got back to you about your follow-up - thanks for pointing out the error in my implementation (I had taken the covariance transform code from the Python Laser Ranging OD example on the main Orekit website).

Things seem to be working now, though not quite as expected. For example, I am often seeing values for the Radial and Cross-Track covariance exceed those of the In-Track, which should never be the case. I can attach more of my code, but first wanted to try out the simpler UnivariateProcessNoise function you mentioned before.

I am currently having trouble even building the object (InvalidArgsError), and am thinking the problem lies in setting up the Hipparchus UnivariateFunctions for lofCartesianOrbitalParametersEvolution and propagationParametersEvolution.

Here is my code:

init_cov = [5000.0, 5000.0, 5000.0, 0.5, 0.5, 0.5, 0.01, 0.01]
i_cov = JArray_double(len(init_cov))
# Initial diagonal covariance into Java
for x,i in enumerate(init_cov):
    i_cov[x] = i
cov_mat = MatrixUtils.createRealDiagonalMatrix(i_cov)

# UnivariateFunction classes for process noise
class f_pos(PythonUnivariateFunction):
    def value(self, x):
        return x**4 / 3
class f_vel(PythonUnivariateFunction):
    def value(self, x):
        return x**2

# Since the paper from before sets this to 0.01, this should just return that
class f_coeff(PythonUnivariateFunction):
    def value(self, x):
        return 0.01

# Orbital Parameters Evolution
orb_param_evol = ArrayList().of_(UnivariateFunction)
for i in range(3):
    orb_param_evol.add(f_pos())
for i in range(3):
    orb_param_evol.add(f_vel())

# Propagation parameters evolution
prop_param_evol = ArrayList().of_(UnivariateFunction)
for i in range(2):
    prop_param_evol.add(f_coeff())

my_uv_proc_noise = UnivariateProcessNoise(cov_mat, LOFType.VNC, PositionAngle.TRUE, orb_param_evol, prop_param_evol)

MaximeJ · February 10, 2020, 9:54am

Hi @aerow610,

Thank you for your update on this question.

Maybe you should report this to the creator of the example, @yzokras.

Regarding your code, beware that the univariate functions in input of the UnivariateProcessNoise class must have the dimension of standard deviations, so you should put in there the sigmas and not the covariance values.
You should have:
return x**2 / sqrt(3) instead of return x**4 / 3 and so on.
I realize it is not clear in the documentation, I’ll open an issue for that in the Orekit issue tracker.

I’m not very good in Python but shouldn’t your variables orb_param_evol and prop_param_evol be instances of ArrayList().of_(PythonUnivariateFunction) instead of ArrayList().of_(UnivariateFunction) ?

I think they should even be JArray_object since UnivariateProcessNoise is awaiting 2 arrays of PythonUnivariateFunction as inputs.

But I did not manage to initialize the JArray_object.
Maybe @petrus.hyvonen could help you on this one.

Cheers,
Maxime

yzokras · February 16, 2020, 11:53pm

Maybe you should report this to the creator of the example, @yzokras.

Thank you for noticing the mistake with the Jacobian multiplication, I fixed it (along with several other issues and improvements) now: Many fixes :) · GorgiAstro/laser-orbit-determination@54fa5bd · GitHub

aerow610 · February 20, 2020, 9:42pm

Hi Maxime,

I switched the instances of orb_param_evol and prop_param_evol as you suggested, yet am still getting the InvalidArgsError. Here is the full traceback:

Traceback (most recent call last):

File “”, line 41, in
my_uv_proc_noise = UnivariateProcessNoise(cov_mat2, LOFType.VNC, PositionAngle.TRUE, orb_param_evol, prop_param_evol)

InvalidArgsError: (<class ‘org.orekit.estimation.sequential.UnivariateProcessNoise’>, ‘init’, (<RealMatrix: Array2DRowRealMatrix{{500.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0},{0.0,500.0,0.0,0.0,0.0,0.0,0.0,0.0},{0.0,0.0,500.0,0.0,0.0,0.0,0.0,0.0},{0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0},{0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0},{0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0},{0.0,0.0,0.0,0.0,0.0,0.0,0.001,0.0},{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.001}}>, <LOFType: VNC>, <PositionAngle: TRUE>, <ArrayList: [org.orekit.python.PythonUnivariateFunction@4b0b12b0, org.orekit.python.PythonUnivariateFunction@32491898, org.orekit.python.PythonUnivariateFunction@2ce7a9de, org.orekit.python.PythonUnivariateFunction@34fdd1d0, org.orekit.python.PythonUnivariateFunction@2770db6d, org.orekit.python.PythonUnivariateFunction@5d078ac3]>, <ArrayList: [org.orekit.python.PythonUnivariateFunction@16165af2, org.orekit.python.PythonUnivariateFunction@371c40ee]>))

I also tried the JArray_object method, but got a TypeError trying to set the elements via:

orb_param_evol = JArray_object(6)
for i in range(3):
    orb_param_evol[i] = f_pos()
for i in range(3:6):
    orb_param_evol[i] = f_vel()

Traceback (most recent call last):

  File "<ipython-input-39-dbfaeeaf2a66>", line 1, in <module>
    orb_param_evol[0] = f_pos()

TypeError: <f_pos: org.orekit.python.PythonUnivariateFunction@3ca53321>

@petrus.hyvonen, is the main error here my implementation of subclassing the PythonUnivariateFunction or does it have to do with the type of arrays being passed to the Orekit UnivariateProcessNoise constructor?

aerow610 · April 14, 2020, 5:46pm

Hi @MaximeJ,

Wanted to revisit this as I still have not had any luck initializing the UnivariateProcessNoise class.

I think the error here may be that I am defining the univariate functions improperly - do you have an example of what one of those functions would look like normally in either python or java?

Thanks!

petrus.hyvonen · April 14, 2020, 7:24pm

Hi,
Sorry lost this thread. Hard to know whats wrong. buts some pointers.

I think it will not matter if you are using .of_(PythonUnivariate…) or its superclass but if you have a list of PythonU… objects that is probably the right thing. Try both…

The f_pos and F_vel looks fine to me.

The problem is likely with casting of the arrays. Try using JArray(‘object’).cast_(yourarray, PythonUnivariateFunction)

See https://lucene.apache.org/pylucene/jcc/features.html

MaximeJ · April 15, 2020, 7:36am

Hi again aerow610,

Petrus, thank you for bumping in, I’m a bit helpless with this kind of advanced Python wrapper usage related question.

I can point you to a Java example in the Orekit test class UnivariateProcessNoiseTest.
Looking at it I’m wondering if you shouldn’t try your luck with objects PolynomialFunction instead of the high-level interface UnivariateFunction. But I’m not sure it will change your issue.

Maxime

aerow610 · May 20, 2020, 6:06pm

@MaximeJ and @petrus.hyvonen - I finally figured it out!

It seems that there was no casting needed, just declaring the JArray('object') from a python list of PythonUnivariateFunction did the trick. Shoutout to Nicolas Mitault for the solution over on the python-wrapper (from about a year ago) https://gitlab.orekit.org/orekit-labs/python-wrapper/-/issues/408

In case anyone else is having a similar issue with creating a JArray('object'), here is what I did (after creating the subclassed PythonUnivariateFunctions f_pos and f_vel):

orb_param_list = [f_pos(), f_pos(), f_pos(), f_vel(), f_vel(), f_vel()]
orb_param_evol = JArray('object')(orb_param_list, PythonUnivariateFunction)

Thanks to both of you for all the help working through this!

petrus.hyvonen · May 20, 2020, 9:05pm

Great you got it to work and thanks for sharing! I think it likely works with the normal UnivariateFunction (without Python) as i understand you are not subclassing.

aerow610 · May 21, 2020, 8:02pm

Actually, for the f_pos() and f_vel() univariate functions, I did subclass (for their value method). Just tested it out and still need the PythonUnivariateFunction at least for the subclassing, however when putting those functions to the JArray('object'), I could specify the type as either PythonUnivariateFunction or UnivariateFunction, and they both worked for creating the UnivariateProcessNoise