Implementing Kalman Filter

[peterdavidgrant]

Good afternoon,

I'm trying to implement a kalman filter to run on my cRIO. I've got a working simulation with a non-linear model in Simulink, using their kalman filter blocks (in continuous time).

State Space Model

I've built a state space model from the non-linear model, and have ended up with (note this is in the attached VI, within the KF block itself):

A = 

     0     1     0     0 

     0     0     0     0 

     0     0     0     1 

     0     0     0     0 

B = 

         0         0 

   -7.0071    0 

         0         0 

         0   -7.0071 

C = 

     1     0     0     0 

     0     0     1     0 

D = 

     0     0 

     0     0 

Q = 

     1     0 

     0     1 

R = 

     1     0 

     0     1 

N =  

    0 

Comparison

I've written some constants to the simulink KF, to get an idea of its response (not for use, but to find out if I'm getting it working in LV):

Note: my R co-variance is outside the KF as it is not time-invariant. 

This gives the following 'characteristic':

LabVIEW

Attached is a simple vi, where I'm attempting to do the same as above (again, in continuous time):

The problem I'm getting is when I run, I get the following error:

Possible reason(s):

Control Design and Simulation: (Hex 0xFFFF5D24) The dimensions of the covariance matrix are improper.

Complete call chain:
NI_CD_Dynamic Analysis.lvlib:cd_Check Covariance.vi
cd_Verify Stochastic State-Space and Noise Models with xhat(0) and P(0).vi
cd_Check Models Change for Continuous Recurisve Kalman Filter.vi
CD Continuous Recursive Kalman Filter.vi:Instance:4:520001
CKF_characteristic.vi

I'm pretty sure I'm inputting my matrices above have been implemented correctly, but its likely I'm doing something wrong here, does anyone have any ideas?

Thanks,

Peter 

[NIRebecca · Accepted solution]

Hi Peter, 

Have you tried editing the size of your Initial Estimation Error Convariance Matrix t0 4x4?

Control and Design User Manual

Thanks,

Rebecca

[peterdavidgrant]

Hi Rebecca,

Thank you for the reply!

Ahh thanks, I thought that configuration parameters like that would have sized automatically, so left it as was, if it was required, so it is now working:

I'm still getting different results to my Simulink model though, I can't work out what else might be different when functionally they should be the same. I'll have a go on the real thing, and see what happens.

I've started digging through the control and design user manual, thanks!

Peter