How were the matrices for A G and C derived in this answer

Hello,

For the Kalman filtering example https://www.mathworks.com/help/control/ug/state-estimation-using-time-varying-kalman-filter.html

I have a quick question regarding how the matrices for A, G and C are derived. I cannot seem to derive the linear equations necessary to generate the matrices listed. I've attempted to generate the matrices by hand by converting from differential equations to state space with no luck. Does any know how those matrices were generated?

Answer 1

[Patrick Saint - Laurent]

The matrix for A is a 4x4 matrix, how does your solution map to A and G?

Answer 2

[Paul]

The matrices are not developed by linearizing and then discretizing the nonlinear differential equation. The doc says that they are simply generated for north and east by

(a) assuming a random walk model for the acceleration: xdothat[n+1] = xdothat[n] + w[n]

(b) trapezoidal approximation to velocity: (xhat[n+1] - xhat[n])/T = (xdothat[n+1] + xdothat[n])/2

sub (a) into (b)

xhat[n+1] = xhat[n] + T/2*(xdothat[n+1] + xdothat[n])

xhat[n+1] = xhat[n] + T/2*(xdothat[n] + w[n] + xdothat[n])

xhat[n+1] = xhat[n] + T*xdothat[n] + T/2*w[n]

so:

[ xhat[n+1] ; xdothat[n+1] ]= [1 T; 0 1] * [ xhat[n] ; xdothat[n] ] + [T/2 ; T] * w[n]

[Paul]

To map this solution to the 4D matrices in that doc page, use the equations above for east and north:

Let x = [xhate xhatn xdothate xdothatn] .'

Let w = [w1 w2] .'

Then using the equations above

xhate[n + 1] = [1 0 T 0] * x + [T/2 0] * w

xhatn[n + 1] = [0 1 0 T] * x + [0 T/2[ * w

xdothate[n + 1] = [0 0 1 0] * x + [1 0] * w

xdothatn[n + 1] = [0 0 0 1] * x + [0 1] * w