Least Squares and Kalman Filtering

Transcription

1 INRODUCION Least Squares and Kalman Filtering R.E. Deain Bonbeach VIC, 396, Australia -Sep-5 he theory of least squares and its application to adjustment of survey measurements is well nown to every geodesist. he invention of the method is generally attributed to Carl Friedrich Gauss ( ) but could equally be credited to Adrien-Marie Legre (75-833). Gauss used the method of least squares to compute the elements of the orbit of the minor planet Ceres and predicted its position in October 8 from a few observations made in the previous year. He published the technique in 89 in heoria Motus Corporum Coelestium in Sectionibus Conicis Solem Ambientium (heory of the Motion of the Heavenly Bodies Moving about the Sun in Conic Sections), mentioning that he had used it since 795, and also developed what we now now as the normal law of error, concluding that: "... the most probable system of values of the quantities... will be that in which the sum of the squares of the differences between the actually observed and computed values multiplied by numbers that measure the degree of precision, is a minimum." (Gauss 89). Legre published an indepent development of the technique in Nouvelles méthodes pour la détermination des orbites des comètes (New methods for the determination of the orbits of comets), Paris, 86 and also as the "Méthod des moindres carriés" (Method of Least Squares), published in the Mémoires de l'institut national des sciences at arts, vol. 7, pt., Paris, 8. After these initial wors, the topic was subjected to rigid analysis and by the beginning of the th century was the universal method for the treatment of observations. Merriman (95) compiled a list of 48 titles, including 7 boos, written on the topic prior to 877 and publication has continued unabated since then. Leahy (974) has an excellent summary of the development of least squares and clearly identifies the historical connection with mathematical statistics, which it predates. he current literature is extensive; the boos Observations and Least Squares (Mihail 976) and Analysis and Adjustment of Survey Measurements (Mihail and Gracie 98), and lecture notes by Cross (99), Kraiwsy (975) and Wells and Kraiwsy (97) stand out as the simplest modern treatments of the topic. Following Wells and Kraiwsy (97, pp.8-9), it is interesting to analyse the following quotation from Gauss' heoria Motus (Gauss, 89, p.49). "If the astronomical observations and other quantities, on which the computation of orbits is based, were absolutely correct, the elements also, whether deduced from three or four observations, would be strictly accurate (so far indeed as the motion is supposed to tae place exactly according to the laws of KEPLER), and, therefore, if other observations were used, they might be confirmed, but not corrected. But since all our measurements and observations are nothing more than approximations to the truth, the same must be true of all calculations resting upon them, and the highest aim of all computations made concerning concrete phenomena must be to approximate, as nearly as practicable, to the truth. But this can be accomplished in no other way than by a suitable combination of more observations than the number absolutely requisite for the determination of the unnown quantities. his problem can only be properly undertaen when an approximate nowledge of the orbit has been already attained, which is afterwards to be corrected so as to satisfy all the observations in the most accurate manner possible."

2 his single paragraph, written over years ago, embodies the following concepts, which are as relevant today as they were then. (i) (ii) Mathematical models may be incomplete, Physical measurements are inconsistent, (iii) All that can be expected from computations based on inconsistent measurements are estimates of the truth, (iv) Redundant measurements will reduce the effect of measurement inconsistencies, (v) Initial approximations to the final estimates should be used, and finally, (vi) Initial approximations should be corrected in such a way as to minimise the inconsistencies between measurements (by which Gauss meant his method of least squares). hese concepts are also embedded in the Kalman Filter, an estimation process developed by Rudolf E. Kalman in 96 (Kalman 96). he Kalman Filter is a set of equations that can be used to determine the best estimates of a set of parameters (the state) lined to a mathematical model of a dynamic measurement system. Kalman s original development was a (linear) solution to non-linear maximum lielihood estimation; developed by R.A. Fisher and studied by Kolmogorov in 94 and Weiner 3 in 94. One of the first implementations of Kalman s filter was in the estimation of a spacecraft s trajectory and it was incorporated into the Apollo navigation computer. It is now a standard component in inertial guidance systems. Kalman filtering is also used in inematic GPS and most modern navigation systems. A Kalman Filter can be thought of as a logical extension of Gauss original development of least squares to estimate unnown parameters of a system. In Gauss era and up until the middle th century, systems (whose parameters were to be estimated) were generally static and measurements unvarying with respect to time. But in this modern era, systems may be dynamic and measurements made from moving platforms at regular time intervals. he estimation process must lin these measurements and the Kalman filter achieves this; estimating the state of a system (the parameters) at intervals of time. hese notes contain derivations of formula and wored examples of least squares estimation (including Kalman filtering). First, there is a general treatment of least squares estimation that is called here Combined Least Squares which can be shown to encompass the usual solutions nown in surveying and geodesy as adjustment of indirect observations and adjustment of observations only. his is followed by the derivation of the Kalman Filter equations using the same basic principles minimizing sum of squares of weighted residuals. he derivations are concise and the interested reader is directed to more extensive developments as references. Sir Ronald Aylmer Fisher (89 96) was an English statistician and mathematician nown for his important contributions to statistics, including the analysis of variance, maximum lielihood, fiducial inference, and the derivation of various sampling distributions. A.N. Kolmogorov (93 987) was a th-century Russian mathematician who made significant contributions to the mathematics of probability theory, topology, classical mechanics and information theory. 3 Norbert Wiener ( ) was an American child prodigy who became a mathematician and philosopher and Professor of Mathematics at MI. Wiener was an early researcher in stochastic and noise processes, contributing wor relevant to electronic engineering, electronic communication, and control systems.

3 COMBINED LEAS SQUARES A common treatment of the least squares technique of estimation starts with simple linear mathematical models having observations (or measurements) as explicit functions of parameters with non-linear models developed as extensions. his adjustment technique is generally described as adjustment of indirect observations (also called parametric least squares). Cases where the mathematical models contain only measurements are usually treated separately and this technique is often described as adjustment of observations only (also called condition equations). Both techniques are of course particular cases of a general technique called here Combined Least Squares, the solution of which is set out below. his general technique also assumes that the parameters, if any, can be treated as observables, i.e., they have an a priori covariance matrix. his concept allows the general technique to be adapted to sequential processing of data where parameters are updated by the addition of new observations. In general, least squares solutions require iteration, since a non-linear model is assumed. he iterative process is explained below. In addition, a proper treatment of covariance propagation is presented and cofactor matrices given for all the computed and derived quantities in the adjustment process. Finally, the particular cases of the general least squares technique are described. Consider the following set of non-linear equations representing the mathematical model in an adjustment ( ˆ, ˆ ) F l x = () where l is a vector of n observations and x is a vector of u parameters; ˆl and ˆx referring to estimates derived from the least squares process such that ˆl = l + v and xˆ = x + δx () where v is a vector of residuals or small corrections and δ x is a vector of small corrections. he observations l have an a priori cofactor matrix Q containing estimates of the variances and ll covariances of the observations. In many cases the observations are indepent and Q ll is diagonal. In this general technique, the parameters x are treated as observables with a full a priori cofactor matrix Q xx. he diagonal elements of Q xx contain estimates of variances of the parameters and the off-diagonal elements contain estimates of the covariances between parameters. Cofactor matrices Q ll and Q xx are related to the covariance matrices Σ ll and Σ xx by the variance factor σ Σ = σ Q Σ = σ Q (3) ll ll xx xx Also, weight matrices W are useful and are defined, in general, as the inverse of cofactor matrices W = Q (4) and covariance, cofactor and weight matrices are all symmetric, hence where the superscript denotes the transpose of the matrix. Q = Q and W = W Note also, that in this development where Q and W are written without subscripts they refer to the observations, i.e., Q = Q and W = W ll ll 3

4 Linearizing () using aylor's theorem and ignoring nd and higher-order terms, gives F ( ˆ F ) ( ) ( ˆ F, ˆ F, ) ( ˆ ) l x = l x + + = ˆl l l xˆ x x l, x l, x (5) and with v = ˆl l and δ x = xˆ x from () we may write the linearized model in symbolic form as Av + Bδx = f (6) Equation (6) represents a system of m equations that will be used to estimate the u parameters from n observations. It is assumed that this is a redundant system where n m u (7) and r = m u (8) is the redundancy or degrees of freedom. In equation (6) the coefficient matrices A and B are design matrices containing partial derivatives of the function evaluated using the observations l and the "observed" parameters x. A F F = B = ˆ ˆ m, n m, u l l, x x l, x he vector f contains m numeric terms calculated from the functional model using l and x. { F ( )} f l x () m, =, HE COMBINED LEAS SQUARES SOLUION he least squares solution of (6), i.e., the solution which maes the sums of the squares of the weighted residuals a minimum, is obtained by minimizing the scalar function ϕ ( ) ϕ = v W v + δx W δx Av + Bδx f () xx where is a vector of m Lagrange multipliers which are at this stage unnown and the is added for convenience later on. ϕ is a minimum when its derivatives with respect to v and δ x are equated to zero, i.e. ϕ ϕ = = and = δ xx = v v W A δ x x W B hese equations can be simplified by dividing both sides by two, transposing and changing signs to give Wv + A = and W x + B = () Equations () can be combined with (6) and arranged in matrix form as xxδ (9) 4

5 W A v = A B f B W xx δ x (3) Equation (3) can be solved by the following reduction process given by Cross (99, pp. -3). Consider the partitioned matrix equation P y = u given as P P y u = P P y u (4) which can be expanded to give P y + P y = u or = ( ) y P u P y (5) Eliminating y by substituting (5) into (4) gives Expanding the matrix equation gives and an expression for y is given by ( ) P P P u P y u = P P y u ( ) P P u P y + P y = u P P u P P P y + P y = u ( ) P P P P y = u P P u (6) Now partitioning (3) in the same way as (4) gives W A v = A B f B W xx δ x (7) and then eliminating v by applying (6) yields B A f A = W A xx δ W B W x [ ] Remembering that Q = W the equation can be simplified as AQA B B W xx f = δ x (8) 5

6 Again, applying (6) to the partitioned equation (8) gives and re-arranging gives the normal equations ( xx ( ) ) δ = ( ) W B AQA B x B AQA f ( ( ) + ) δ = ( ) xx B AQA B W x B AQA f (9) Mihail (976, p. 4) simplifies (9) by introducing equivalent observations l e where l = A l () e Applying the matrix rule for cofactor propagation (Mihail 976, pp ) gives the cofactor matrix of the equivalent observations as Q = e AQA () With the usual relationship between weight matrices and cofactor matrices, [see (4)], we may write Using () in (9) gives the normal equations as with the auxiliaries N and t as e ( ) W = Q = AQA () ( ) e B W B + W δx = B W f (3) e xx e N = B WeB t = B Wef (4) he vector of corrections δx is given by ( ) δx = N + W t (5) he reduction process applied to (3) also yields the vector of Lagrange multipliers ( ) ( δ ) e ( δ ) and the vector of residuals v is obtained from () as xx = AQA f B x = W f B x (6) v = W A = QA (7) HE IERAIVE PROCESS OF SOLUION Remembering that xˆ = x + δx, where x is the vector of a priori estimates of the parameters, δx is a vector of corrections and ˆx is the least squares estimate of the parameters. At the beginning of the iterative solution, it can be assumed that ˆx equals the a priori estimates x and a set of corrections δx computed. hese are added to x giving an updated set x. A and B are recalculated and a new weight matrix W xx computed by cofactor propagation. 6

7 he corrections are computed again, and the whole process cycles through until the corrections reach some predetermined value, which terminates the process. ˆ = + δ n + n n x x x (8) COFACOR MARICES Derivation of the cofactor matrices is a lengthy process and the results given below can be found in Mihail (976, pp ) Cofactor Matrix for ˆx = ( + ) Cofactor Matrix for ˆl ( ) ll ˆˆ Q N W (9) xx ˆˆ xx Q = Q + QA W B N + W B W AQ QA W AQ (3) e xx e e Cofactor Matrix for δx = ( + ) Q N W NQ (3) δxδ x xx xx Cofactor Matrix for v Covariance Matrix Σ xx ˆˆ he estimated variance factor is where the degrees of freedom r are σ Q = Q Q (3) vv xx ˆˆ ll ˆˆ Σ = σ Q (33) + δ r xx ˆˆ δ xx = v Wv x W x (34) r = m u + ux (35) m is the number of equations used to estimate the u parameters from n observations. u x is the number of weighted parameters. [(35) is given by Kraiwsy (975, p.7, eq. -6) who notes that it is an approximation only and directs the reader to Bossler (97) for a complete and rigorous treatment.] GENERAION OF HE SANDARD LEAS SQUARES CASES Combined Case with Weighted Parameters ( A; B; W; W xx ) with and xx Av + Bδ x = f W = Q W he general case of a non-linear implicit model with weighted parameters treated as observables is nown as the Combined Case with Weighted Parameters. It has a solution given by the following equations. ( ) δx = N + W t (36) with ( = = = = ) e e e e xx N B W B t B W f W Q AQA (37) xˆ = x + δx (38) 7

8 ll ˆˆ e ( δ ) = W f B x (39) v = W A = QA (4) xx ˆˆ ˆ l = l + v (4) ( ) Q = N + W (4) ( ) e xx e e xx Q = Q + QA W B N + W B W AQ QA W AQ (43) Q = Q Q (44) vv ll ˆˆ v Wv + δx Wxxδ x v Wv + δx Wxxδ x σ = = r m u + u x (45) Σ = σ Q Σ = σ Q Σ = σ Q (46) xx ˆˆ xx ˆˆ vv vv ll ˆˆ ll ˆˆ Combined Case with A ; B ; W ; W = xx Av + Bδ x = f with W = Q and Wxx = he Combined Case is a non-linear implicit mathematical model with no weights on the parameters. he set of equations for the solution is deduced from the Combined Case with Weighted Parameters by considering that if there are no weights then W = and Q =. his implies that x is a constant vector (denoted by x ) of approximate values of the parameters, and partial derivatives with respect to x are undefined. Substituting these two null matrices and the constant vector x = x into equations (36) to (45) gives the following results. δ xx xx x = N t (47) with ( ) ( ) N = B W B t = B W f f = F x, l W = Q = AQA (48) e e e e ˆ x = x + δx (49) e ( δ ) = W f B x (5) v = W A = QA (5) ˆ l = l + v (5) Qδx δx = Qxx ˆˆ = N (53) Q = Q + QA W B N B W AQ QA W AQ (54) ll ˆˆ e e e 8

9 Q = Q Q (55) vv ll ˆˆ v Wv v Wv σ = = r m u (56) Σ = σ Q Σ = σ Q Σ = σ Q (57) xx ˆˆ xx ˆˆ vv vv ll ˆˆ ll ˆˆ Indirect Least Squares (Parametric Case) with A = I; B; W; Wxx = v + Bδ x = f W = Q W = with and xx Indirect Least Squares (Parametric Case) is a mathematical model with the observations l explicitly expressed by some non-linear function of the parameters x only. his implies that the design matrix A is equal to the identity matrix I. Setting A = I in the Combined Case (with no weights) leads to the following equations. with = = = F (, ) δ x = N t (58) N B WB t B Wf f x l (59) ˆ x = x + δx (6) v = f Bδx (6) ˆ l = l + v (6) Qδx δx = Qxx ˆˆ = N (63) Qvv = Q BN B (64) Qll ˆˆ = B N B (65) v Wv v Wv σ = = r n u (66) Σ = σ Q Σ = σ Q Σ = σ Q (67) xx ˆˆ xx ˆˆ vv vv ll ˆˆ ll ˆˆ 9

10 Observations Only Least Squares (Condition Case) with A; B = ; W; Wxx = Av = f W = Q W with and xx Observations Only Least Squares (Condition Case) is characterized by a non-linear model consisting of observations only. Setting B = in the Combined Case (with no weights) leads to the following equations. with = = ( ) = ( ) = W f (68) e e F e W Q AQA f l (69) v = W A = QA (7) ˆ l = l + v (7) Qll ˆˆ = Q QA We AQ (7) Q = Q Q (73) vv ll ˆˆ v Wv σ = = r v Wv m (74) Σ = σ Q Σ = σ Q (75) vv vv ll ˆˆ ll ˆˆ EXAMPLES he following simple examples of least squares solutions (or least squares estimations) show how appropriate mathematical models are developed and systems of (matrix) equations solved to give estimates of parameters and/or residuals. MALAB functions are given in the Appix and show how these solutions may be programmed. Examples and are both fitting straight lines y = b x + c through data points. his is also nown as linear regression. Example assumes that the x-values are error free and the y-values are the measurements (subject to error) with associated residuals and weights reflecting the precision. he least squares solution for parameters b (slope) and c (y-intercept) is obtained using Indirect Least Squares (Parametric case). Example assumes that both the x and y-values are measurements (with associated residuals) with estimates of precision (variances and covariances). he least squares solution for the parameters b and c uses Combined Least Squares. his technique is rarely explained in textboos on the topic. Example 3 is the solution of a small level networ using two methods; Observations Only Least Squares (Condition case) and Indirect Least Squares (Parametric case) Example 4 is position fix from measured distances to beacons of nown coordinates using Indirect Least Squares

11 Example : Line of Best Fit y 5 C 3 4 y = b x + c x Point x (mm) y (mm) weight w Figure. Line of Best Fit through data points to 5 he line of best fit shown in the Figure has the equation y = b x + c where b is the slope of the y y line b = tan θ = and c is the intercept of the line on the y axis. x x b and c are the parameters and the data points are assumed to accord with the mathematical model y = b x + c and the x,y coordinate pairs of each data point are considered as indirect measurements of the parameters m and c of the mathematical model. he column of weights reflects the differing precision associated with the measured y-values (large weight equals small precision) and the x- values are considered to be error free. Now, since the x-values are error-free, the residuals v are associated with the measured y-values only, which leads to an observation equation of the form y + v = b x + c (76) his equation can be re-arranged into a form where the unnowns (v, b, c) are on the left-hand side of the equals sign and the nowns (y) are on the right-hand side For the 5 data points there are n = 5 equations in u = parameters v b x c = y (77) v x b c = y v x b c = y v x b c = y v x b c = y v x b c = y (78)

12 hese can be written in the matrix form v( ) + B( ) x n, n, u ( u, ) = f ( n,) (Parametric Case: A = I; B; W; Wxx = ) as v x y v x y b v 3 + x3 y 3 c = v x y v 5 x5 y 5 (79) he numerical values in the matrices B, f and W are B(, ) = n u f( n, ) = W ( n, n) = he solution is given by equations (58) to (67). In this particular case, the solution for the vector x (containing the parameters b and c) is direct; no iteration and no approximate values required. he solution is given by N( ) = B, (, ) W(, ) B(, ) = u u u n n n n u (,) = u ( u, n) ( n, n) ( n,) = 3.. t B W f 7. b e e = c = = = 5.699e e and x( ) N u, ( u, u) t ( u,) he slope of the line b = tan θ = and θ = 3 4 axis and the line cuts the y-axis at.6693 measured anticlocwise from the x he residuals (mm) are v ( n,) = A MALAB program least_squares.m is given in the Appix and can be used to solve parametric least squares problems given the coefficient matrix B, the vector of numeric terms f and a vector of weights w that are the elements of a diagonal matrix W. his program requires a data file (an ASCII text file) with an extension.txt containing the elements of B, f and w. he output from the program containing estimates of parameters and residuals and relevant cofactor matrices is placed in a text file with the extension.out in the same directory as the data file. he Appix shows the input and output files Example_.txt and Example_.out

13 Example : Line of Best Fit correlated data of varying precision y 5 C 3 y = b x + c 4 x Point x (mm) y (mm) Figure. Line of Best Fit through data points to 5 that have varying precision he line of best fit shown in the Figure has the equation y = b x + c where b is the slope of the y y line b = tan θ = and c is the intercept of the line on the y axis. x x b and c are the parameters and the data points are assumed to accord with the mathematical model y = b x + c and the x,y coordinate pairs of each data point are considered as indirect measurements of the parameters b and c of the mathematical model. he data points in Figure have varying precision indicated by error ellipses and the size, shape and orientation of the error ellipses are functions of the variances and covariance of the coordinates at each point. As a general rule, a point with a small error ellipse is more precisely located than a point with a large error ellipse. he cofactor matrix Q of the n = measurements (the coordinates) has the following form Where x y Q n, n x y y ( ) s, s are estimates of the variances sx s x y sx y s y sx s x y = s s sx s 5 x5 y5 sx 5 y s 5 y5 σ x and σ y respectively and covariance σ. s, s are estimates of the standard deviations σ, xy positive square root of the variance. x y x s x y is an estimate of the σ and standard deviation is the y 3

14 Note that correlation s xy ρ xy = and ρ xy sx sy he actual numeric values (mm ) for each point are: Q ( n, n) = Assuming that residuals v x and v y are associated with both the x and y measurements (the coordinate pairs) the observation equation is ( ) y + v = b x + v + c (8) y x Let = + δ (8) b b b where b is an approximate value and δ b is a small correction and substituting (8) into (8) gives ( δ )( ) y + v = b + b x + v + c y x = b x + b v + x δb + v δb + c (8) x x But, since v x and δ b are both small, then the product vx δb in (8) and we write the observation equation as y + v = b x + b v + x δb + c (83) y x Re-arranging the observation equation so that unnown quantities are on the left-hand-side and nown quantities are on the right-hand-side of the equals sign gives b v + v x δb c = b x y (84) x y For the 5 data pairs ( n = observations), the ( m = 5 ) observation equations for the ( u = ) parameters are 4

15 b v + v x δb c = b x y x y b v + v x δb c = b x y x y b v + v x δb c = b x y x3 y b v + v x δb c = b x y x4 y b v + v x δb c = b x y x5 y hese can be written in the matrix form A( ) v( ) + B,, (, ) δ x m n n m u ( u, ) = f ( m,) (Combined Case: A ; B ; W ; W = xx ) as vx v y v x b x b x y vy x b b x v x3 δ b y b + x3 v y3 c = b x3 y3 b x4 v b x4 y4 x4 b x5 v b x y4 5 y5 v x5 vy5 he least squares solution for the parameters δ b and c and the related precision estimation are given by equations (47) to (57). he solution is iterative, terminating when δ b reaches a sufficiently small value. For a first iteration with an approximate value and f are A ( m, n) b =.55 the numerical values in the matrices A, B = B( m, u) = f ( m,) = ( ) and the equivalent weight matrix W ( ) = A( ) Q e m, m m, n ( n, n) A ( n, m) is 5

16 W (, ) e m m = he matrices N and t are N( ) = B( ) W ( ) B,,, (, ) = u u u m e m m m u t( u, ) = B( u, m) We ( m, m) f( m,) = he solutions are δ x = N t = = and ( u, ) ( u, u) ( u,) δ b c = + δ = b b b A second iteration with b = gives the solutions as δ δ b c x = N t = = and ( u, ) ( u, u) ( u,) A third iteration with b = gives the solutions as δ x = N t = = and ( u, ) ( u, u) ( u,) δ b.455 c = + δ = b b b = + δ = b b b A fourth (and last) iteration with b = gives the solutions as δ δ b.58 c x = N t = = and ( u, ) ( u, u) ( u,) vx.53 v.645 y v x 7.79 vy 6.33 v x and the residuals v( n, ) = Q( n, n) A( m, n) ( n,) = = vy v x4.5 vy4.8 v x5. vy5.8 = + δ = b b b 6

17 A MALAB program linear_regression_cls.m is given in the Appix that solves Linear Regression problems using Combined Least Squares. his program requires a data file (an ASCII text file) with an extension.txt containing data for each point (and there should be at least three points). each line of the data file contains a point number, x-coordinate, sx (standard deviation of the x-coordinate), y-coordinate, sy (standard deviation of the y-coordinate) and sxy (the covariance between the x and y coordinates). he output from the program containing estimates of parameters and residuals and relevant cofactor matrices is placed in a text file with the extension.out in the same directory as the data file. he Appix shows the input and output files Example_.txt and Example_.out Example 3: Level Networ Adjustment PM 73 RL A B 3 C RL 3.66 PM 79 Line Height diff s.d Figure 3. Level Networ Figure 3 is a schematic diagram of a small level networ connecting points A, B and C to PM s 79 and 73 of nown Australian Height datum (AHD) Reduced Levels (RL s). On the diagram, the arrows indicate the direction of rise; i.e., A is lower than PM79 and C. And B is higher than A, C and PM73. he height differences and standard deviations (metres) are shown in the table to the right of the diagram. We will adjust this level networ using two different methods; firstly using Observations Only least squares (Condition case) and secondly using Indirect least squares (Parametric case). Observations Only Least Squares here are n =5 observations (measured height differences) and a minimum of n = 3 observations are required to fix the RL s of A, B and C. Hence there are m = n n = redundant measurements, which equals the number of indepent condition equations. Denoting the observations as l, l, etc. these two conditions are ( ) ( ) ( ) l + v l + v + l + v = RL RL l + v + l + v l + v = hese equations may be re-arranged with the residuals on the left-hand side of the equals sign and numeric values on the right-hand side ( ) ( ) v v + v =.53 l l + l v + v v = l + l l (85) 7

18 he condition equations in matrix form A( ) v, (,) = f m n n ( m,) are v v.5 v 3 =. v 4 v 5 (86) he cofactor matrix Q containing estimates of variances and covariances is (upper-triangular part) ( ).5 (.) Q ( n, n) = (.) (87) (.) (.5) he solution for the residuals v, adjusted observations ˆ l = l + v and cofactor matrices Q ˆˆ and Q ll vv are given by equations (68) to (75). Using (86) and (87) the relevant equations are ( ) W ( ) = A( ) Q,, (, ) A(, ) = e m m m n n n n m ( m, ) = We ( m, m) f( m,) = v(,) = Q ˆ (, ) A(, ) n n n n m ( m, ) =.39 l( n, ) = l( n, ) + v ( n,) =.658 (89) he adjusted RL s are: A =.9 m, B = 3.76 m, C =.67 m. he cofactor matrices of the adjusted height differences and residuals are (upper-triangular part).333e e e-7.658e-6.867e-5.639e-6.367e-6.658e e-7 Q ˆˆ =.639e-6.658e e-7 (9) ll.536e-6.658e-6.333e-5 (88) 8

19 .867e e e-7.658e-6.867e-5.367e-6.367e-6.658e e-7 Q vv =.367e-6.658e e-7 (9).4684e-6.658e-6.867e-5 he variance factor is v Wv σ = = m m (9) Precision estimates of the adjusted RL s of A, B and C can be made in the following way. he RL s are obtained from adjusted observations as lˆ RL ˆ ˆ ˆ A = l5 l4 + RL73 RL A l RL73 RL ˆ ˆ B = l5 + RL 73 or RL B l RL = RL ˆ ˆ RL ˆ RL C = l5 l3 + RL 73 C l 73 4 lˆ 5 (93) hese equations can be written in a matrix form y = Cl ˆ + d and using Propagation of Variances for linear functions we may write the cofactor matrix of the parameters in y (the adjusted RL s) as Q yy = CQll ˆˆC (94) where Q yy contains estimates of the variances ( A, B, C ) s s s and covariances (,, ) s s s of the AB AC BC adjusted RL s. with C given in (93) and Q s s s = A AB AC yy sba sb sbc sca scb sc Q ll ˆˆ given in (9) the cofactor matrix of the adjusted RL s Q yy in (94) is.339e e-5.5e-5 Q yy =.8679e-5.339e-5.5e-5 (95).5e-5.5e-5.45e-5 Using the relationship σ Σ = Q and (95) the standard deviations of the adjusted RL s are A B A B C ( )( ) σ = σ s = e-5 =.74 m ( )( ) σ = σ s = e-5 =.74 m ( )( ) σ = σ s = e-5 =.7797 m C 9

20 Indirect Least Squares he observation equation for a measured height difference l XY between two points X and Y can be written as RLX + lxy + vxy = RLY (96) Using (96) we may write an equation for each of the n = 5 observations (measured height differences) in the form RL RL RL RL RL A C A A + l + v = RL 79 + l + v = RL + l + v = RL l + v = RL l + v = RL hese equations may be re-arranged so that the unnown quantities (the residuals and the u = 3 unnown RL s of A,B,C) are on the left-hand side of the equals sign and the nown quantities are on the right-hand side v + RL + RL + RL = RL l A B C 79 v + RL + RL = l v A C RL + RL = l 3 B C 3 v + RL RL = l v 4 A B 4 C B B B ( RL l ) RL = + 5 B 73 5 In the matrix form v( ) + B( ) x n, n, u ( u, ) = f ( n,) the equations are v. v RL A.45 v 3 + RL B =.655 v RL.7 4 C v (97) he estimates of the variances and covariances are contained in Q [see (87)] and the diagonal weight matrix W is 4 5 W Q (98) = = 5 ( n, n) ( n, n) 5 4 he solution for the vector x (the three RL s), the residuals v and cofactor matrices Q xx and Q vv are given by equations (58) to (67) and these are embodied in the MALAB function least_squares.m given in the Appix, and a suitable ASCII text file for this problem is

21 he solutions are Data file for function "least_squares.m" Example 3: Level Networ B() B() B(3) f w N( ) = B( ) W( ) B( ) = u, u u, n n, n n, u t(,) = B(, ) W(, ) f(,) = 365 u u n n n n e-5.867e-5.5e x(,) = N(, ) t(,) =.867e-5.333e-5.5e u u u u =.5e-5.5e-5.45e v(,) = f(,) B(, ) x(,) =.39 n n n u u he cofactor matrix sa sab s AC.333e-5.867e-5.5e-5 Qxx = sba sb s BC.867e-5.333e-5.5e-5 = N = sca scb s C.5e-5.5e-5.45e-5 and the cofactor matrices Q ˆˆ and Q ll vv are given in (9) and (9). he variance factor is Using the relationship xx xx σ = v Wv m n u = Σ = σ Q the standard deviations of the adjusted RL s of A, B and C are A B A B C ( )( ) σ = σ s = e-5 =.74 m ( )( ) σ = σ s = e-5 =.74 m ( )( ) σ = σ s = e-5 =.7797 m C

22 Example 4: Position Fix by measured distances B A 3 true path - C Figure 4. Path of a ship in a navigation channel Figure 4 shows the path of a ship in a navigation channel as it moves down the shipping channel at a constant heading and speed. Navigation equipment on board automatically measures distances to transponders at three navigation beacons A, B and C at 6-second intervals. he measured distances are nown to have a standard deviation of metre and the solid line in Figure 4 represents solutions of the ship's position for each set of measurements at the 6-second time intervals. he true path of the ship is shown as the dotted line. he coordinates of the three navigation beacons are: A:. E B: 388. E C: 555. E. N 5. N 76. N When the ship was at position the measurements to the beacons were: A: m B: m C: 77. m And the approximate location of the ship at position is: E, N. Indirect Least Squares can be used to determine the best estimate of the ship at position by an iterative technique set out as follows. Observation equation for a measured distance he observation equation for a measured distance at position to beacon j can be written as l + v = lˆ (99) j j j where l j are the measured distance, v j is the residual (small unnown correction) and l ˆj is the least squares estimate. =,,3, is the ship location and j = A,B,C are the beacons. he estimates l ˆj are non-linear functions of the beacon coordinates E, N and the ship estimates Eˆ, N ˆ j j

23 (,,, ) ( ) ( ) lˆ = lˆ Eˆ Nˆ E N = Eˆ E + Nˆ N () j j j j j Expanding () into a series using aylor s theorem gives ˆ ˆ ˆ l ( ˆ l l = l + E ) ( ˆ E + N N ) + higher-order terms Eˆ Nˆ () where E, N are approximate coordinates of the ship at position, l is an approximate distance computed using E, N and the coordinates of the beacon, and the partial derivatives are d j lˆ Eˆ E ˆ E j l N N j = = dj ; = = cj for j = A, B, C l Nˆ l j, c are dimensionless quantities nown as distance coefficients. j With Ê = E + δ E and ˆN = N + δ N where δ E, δ N are small corrections then Ê E = δ E and ˆN N = δ N. hese expressions can be substituted into () ignoring higher-order terms giving a aylor series approximation for ˆl for a single distance Re-arranging (99) for a single distance gives j () ˆl = l + d δ E + cδ N (3) v lˆ = l (4) and substituting (3) into (4) and re-arranging gives the linearized form of the observation equation for a measured distance as Matrix form of observation equations v d δ E c δ N = l l (5) j j j j j Observation equations for each measured distance to beacons j = A, B, C from location = can be written in matrix form as va d A ca l A la δ E v d c l l + = v d c l l he n = 3 equations in u = unnowns ( E, N ) B B B B B δ N = C C = C = C C = δ δ can be written as ( n, ) ( n, u) ( u, ) ( n,) (6) v + B δ x = f (7) he Indirect Least Squares solution for the vector of corrections δ x is given by equations (58) to (67). he approximate coordinates are updated by adding the corrections and a new iteration (new equations (6) formed and solved) performed. he iterative process is terminated when the corrections reach some pre-determined (small) value. 3

24 Iterative solution. Coordinates, measured distances and standard deviations Station East (m) North (m) Observed distance l (m) s.d. (m) E = N = A B C Computed bearings and distances, distance coefficients, numeric terms (comp obs) Station Bearing (degrees) Distance l (m) E E d = l N N c = l l l A B C Solution for small corrections δ E, δ N he solution for the vector δ x (the corrections δ E, δ N ), the residuals v and cofactor matrices Q xx and Q vv are given by equations (58) to (67) and these are embodied in the MALAB function least_squares.m given in the Appix, and a suitable ASCII text file for this problem is With W = Data file for function "least_squares.m" Example 4: Position Fix by distances B() B() f w I the solutions are N( ) = B( ) W( ) B( ) = u, u u, n n, n n, u t(,) = B(, ) W(, ) f u u n n n ( n,) = δ E δ x( u, ) = N( u, u) t( u,) = = δ N v(,) = f(,) B ˆ (, ) x(,) =.7756 l(,) = l(,) + v (,) = n n n u u n n n

25 he cofactor matrix Qxx = N (see above) and the cofactor matrices Q ˆˆ and ll vv Q are Q Q vv ll ˆˆ.7848e- 4.8e-.7933e- = 6.64e-.6455e-.548e- 7.5e- 4.8e-.7933e- = e-.6455e e- he variance factor σ = v Wv.8589 m n u = 4. Update coordinates E = E + δ E = = m N = N + δ N = = m 5. Perform next iteration It can be shown that using the updated coordinates in a new iteration yields corrections having magnitudes less than.5 m. So the values E = m, N = m can be regarded as correct to the nearest mm. Precision of Position Fix he variance-covariance matrix σ E σ EN Σ xx = σ = Qxx = σ N = σ (8) EN σ N contains the variances and covariances of the adjusted coordinates of point. he standard deviations of the east and north coordinates are σ =.8889 =.939 m E σ = =.79 m N 5

26 Standard Error Ellipse Consider a point whose variances σ E, σ N and covariance σ EN are nown. he variance in any other direction u may be calculated by considering the projection of E and N onto the u-axis which is rotated anti-clocwise from the E-axis by an angle φ. u = E cosφ + N sinφ N cosφ E = sinφ N (9) u or y = Ax () φ E cos φ Figure 5. N sin φ E Applying Propagation of Variances to () gives Σ yy = A Σ xxa or cosφ σ cos E σ EN φ σ u = σ cos sin cos sin E φ σ N φ σ EN φ φ sinφ = + + σ sinφ EN σ N () Equation () gives the variance σ u in a direction φ (positive anti-clocwise) from the E-axis and defines the pedal curve of the Standard Error Ellipse. N P A tangent minor axis O normal major axis φ θ a E b Ellipse Pedal curve Scale of units Figure 6. he pedal curve of the Standard Error Ellipse In Figure 6, A is a point on an ellipse. he tangent to the ellipse at A intersects a normal to the tangent passing through O at P. As A moves around the ellipse, the locus of all points P is the pedal curve of the ellipse. he distance of σ u define the direction and lengths of the axes of the ellipse. OP = σ u for the angle φ. he maximum and minimum values 6

27 he semi-axes a and b of the Standard Error Ellipse are the maximum and minimum values of σ u and the angle (positive anti-clocwise) from the E-axis to the major axis of the Standard Error Ellipse is θ and σ tan θ = σ ( σ E σ N ) ( σ EN ) ( σ E σ N W ) ( σ E σ N W ) W = + a = + + b = + EN E σ N () Using the values in (8) σ =.8889, σ =.38986, σ =.789 and using () gives E N EN the parameters of the Standard Error Ellipse for the position fix at point as W = a = m b = m tan θ =.583 θ = degree θ = degree B A 3 - C Figure 7. Standard Error Ellipse of position fix 7

28 Exercise : Line of Best Fit Figure 8 shows part of an Abstract of Fieldnotes with offsets from occupation to a traverse line in Whitten Street. he bearing of the traverse line is 9. post & wire B.4 ABSRAC OF FIELDNOES Distances in metres post & wire 5.34 post & wire.8 post & wire 5.44 post and wire post & wire A N WHIEN ROAD Figure 8. raverse and offsets in Whitten Road Use Indirect least squares (and least_squares.m) to determine the bearing of the line of best fit through the occupation in Whitten Street. You may consider the chainages (linear distances along Whitten Street) to be free of error. ( he answer is: ) Exercise : Index Error of otal Station EDM Six horizontal distances between four points in a straight line are measured with a otal Station that is nown to have an index correction c. A B C D x y z Figure 9. otal Station baseline he otal Station measurements are: AB 5.98 m BC.745 m CD 3.65 m AC 6.9 m AD m BD 5.3 m he measurements are assumed to be of equal precision (i.e., W = I ). he measurement model is assumed to be: observation + residual + c = true value Use Indirect least squares (and least_squares.m) to determine the index correction c and the distances x, y and z. ( he answers are: c = -.44, x = 5.58 m, y =.75 m, z = m ) 8

29 Exercise 3: Parabolic Vertical Curve A surveyor woring on the re-alignment of a rural road is required to fit a parabolic vertical curve such that it is the best fit to the series of natural surface Reduced Levels (RLs) on the proposed new alignment. Figure shows a vertical section of the proposed alignment with Chainages (x-values) and RLs (y-values). Natural rf u a S c e Datum RL 5. Red. Level Chainage Figure. Natural surface vertical section he general equation of a parabolic curve is y = ax + bx + c he chainages are assumed to be error free and the RL s are of equal precision. Use Indirect least squares (and least_squares.m) to determine the values of the coefficients a and b and the constant term c. ( he answers are: a =.5, b = -.688, c = 6.35 ) Exercise 4: Position Fix In Example 4 (see Figure 4) the position of the ship in the navigation channel is obtained using Indirect least squares to solve for corrections to approximate coordinates of the ship when the measurements to shore-based beacons are made. In Example 4 the ship is at position. For this Exercise, solve for the ship s position at point 3 when the measured distances are 3 A: m 3 B: 687. m 3 C: m And the approximate location of the ship at position 3 is: E, N. Only a single iteration is required. ( Answer: E = m, N = m ) 9

30 HE KALMAN FILER A Kalman filter is a set of equations that are applied recursively to estimate the state of a system from a sequence of noisy measurements at times t, t, t 3, etc. he state of the system is its value or values at times t, t, t 3, etc. and a system may have a single value or multiple values. Say, for instance, the system is a ship steaming on a particular heading in a shipping channel and the state of, E, N We say that the system (the ship) is its east and north coordinates ( ) E N and its velocity ( ) this system (the ship) has a state vector x = E, N, E, N containing four elements and the subscript indicates a value at time t. On the other hand, a system may be a process such as electronic distance measurement (EDM) by phase comparison of emitted and reflected light beams. he state of this system is a single value, the distance ( ) = [ D ] D, determined at times t, t, t 3, etc., and this system (the EDM) has a state vector x containing a single element and the subscript indicates a value at time Noisy measurements are measurements that contain small random errors assumed to be normally distributed, i.e., the aggregation of errors in size groupings would form the familiar symmetric bellshaped histogram with positive and negative errors equally liely and small errors more frequent than large errors. Surveyors usually tal of residuals (or corrections) rather than errors, where a residual is the same magnitude as an error but of opposite sign. A Kalman filter gives the best estimates of the state of a dynamic system at a particular instant of time. And a dynamic system can be one whose values are changing with time, due to the motion of the system and measurement errors, or one whose values are measured at various instants of time and appear to change due to measurement errors. Dynamic systems do not have a single state (consisting of one or many values) that can be determined from a finite set of measurements but instead have a continuously changing state that has values sampled at different instants of time. he Kalman filter equations were published in 96 by Dr. R.E. Kalman in his famous paper describing a new approach to the solution of linear filtering and prediction (Kalman 96). Since that time, papers on the application of the technique have been filling numerous scientific journals and it is regarded as one of the most important algorithmic techniques ever devised. It has been used in applications ranging from navigating the Ranger and Apollo spacecraft in their lunar missions to predicting short-term fluctuations in the stoc maret. Sorenson (985) shows Kalman's technique to be an extension of C.F. Gauss' original method of least squares developed in 795 and provides an historical commentary on its practical solution of linear filtering problems studied by th century mathematicians. he derivation of the Kalman filter equations can be found in many texts related to signal processing that is the usual domain of Electrical Engineers, e.g., Brown and Hwang (99). hese derivations often use terminology that is unfamiliar to surveyors, but two authors, Kraiwsy (975) and Cross (99) both with geodesy/surveying bacgrounds, have derivations, explanations, terminology and examples that would be familiar to any surveyor. his paper uses terminology similar to Cross and Kraiwsy. he Kalman filter equations and the associated measurement and dynamic models are given below with a brief explanation of the terms. It is hoped that the study of the two examples will mae help to mae the Kalman filter a relatively easily understood process. t. 3

31 In the derivation and explanation that follows the hat symbol (^) above a vector x indicates that it is an estimate of the true (but unnown) state of the system derived from the Kalman Filter (a least squares process). his is also nown as the filtered state. he prime symbol ( ) indicates a predicted quantity. Equations mared on the left-hand side are components of the Kalman Filter and are listed in the summary of the filter at the of the derivations. Primary and Secondary (or Dynamic) Measurement Models Suppose that x, x, x3,, x, x are vectors of parameters or state vectors of a system at times t, t, t3,, t, t and that l, l, l 3,, l, l are the corresponding vectors of measurements associated with the parameters. We may write three equations as follows: v + B x = f v + B x = f m primary t primary t x = x + v secondary or dynamic (3) where x is the state vector containing the parameters of the system v is the vector of residuals associated with the measurements l where ˆ l = l + v B is a coefficient matrix f is a vector of numeric terms derived from the measurements l is the transition matrix is a vector of residuals associated with the dynamic model vm he primary measurement models in (3) lin measurements l (contained in the vector of numeric terms f ) with parameters in the state vector x at times t and t. he primary measurement model is the same as the Parametric case in the earlier Combined Least Squares derivations and examples [see Example : Line of Best Fit and Example 3: Level Networ Adjustment (Indirect Least Squares)] he secondary or dynamic model in (3) lins the state vectors x at times t and t. he transition matrix is an attempt to model temporal changes between the state vectors (the dynamics of the system) and vm is a vector of corrections reflecting the fact that the transition matrix is an approximation of the true dynamics. he elements of vm are assumed to be small, random and normally distributed with a mean of zero. he measurements W, W and m l and l and the model corrections vm have associated weight matrices W and cofactor matrices, Q Q and Q m where in general Q = W. 3

32 System Driving Noise of the Secondary Model For the solution of many practical problems, it is useful to assume the vector v m as being the product of two matrices v = m Hw (4) where w is a vector of quantities nown as the system driving noise which cause the secondary model to be incorrect and H is a coefficient matrix chosen so that the product Hw represents the effect of these quantities on the parameters. Note that in general H will not be a square matrix as the number of error sources causing the system noise in the secondary model is not necessarily equal to the number of parameters in x. he Cofactor Matrix of the Secondary Model Qm he system driving noise w in (4) is assumed to be a vector of random quantities with zero mean and variance matrix estimated by the cofactor matrix Q. he cofactor matrix Qm can be obtained using Propagation of Variances (or propagation of cofactors) that can be summarized as follows: If linear (or linearized) equations can be expressed in a matrix form w y = Ax + b where y is a vector of variables, A is a coefficient matrix, x is a vector of variables having an associated cofactor matrix Q x and b is a vector of constants then the cofactor matrix of the variables y is given by Cofactor propagation using (4) gives Q y = AQ A and the weight matrix of the secondary model is given by Derivation of the Kalman Filter Equations x Qm = HQwH (5) ( ) ( ) W = Q = HQ H (6) m m w he system of equations for the Kalman Filter are the primary models at t and t, and the secondary model are v + B x = f v + B x = f x + x v = m (7) Enforcing the least squares principle the sum of the squares of the residuals, multiplied by coefficients reflecting their precision ( v Wv ) leads to minimizing the function ϕ where 3

33 = v W v + v W v + v W v ϕ m m m ( ) ( ) ( ) v + B x f v + B x f x x v 3 m (8) Note that there are three quadratic forms ( v Wv ) to be minimized subject to three constraint equations. ϕ can be minimized by using Lagrange s method of undetermined multipliers,, 3 and setting the partial derivatives of (8) to zero. ϕ ϕ = v W = = v W = v v ϕ ϕ = v W + = = B + = v m m m 3 3 x ϕ = B 3 = x ransposing, re-arranging and dividing each equation by gives W v = W v = m W v + = m B + = 3 3 B = 3 hese five equations together with the primary and secondary models form a system of normal equations that can be written in the form of a hyper-matrix W I v W I v W m I vm B = B I I B 3 f I B x f I I x (9) Partial Solution x Using only the observations l at time t a partial solution for x designated x can be obtained from (9) by deleting all matrices associated with the primary model at t and the secondary model. his gives 33