Method and device for three-weight message-passing optimization scheme

1. A method, comprising: (a) receiving an optimization problem, the optimization problem including a plurality of cost functions and a plurality of variables, the cost functions representing possible costs for values of the variables in the optimization problem, each of the cost functions having a predetermined relationship with select ones of the variables, wherein the optimization problem comprises a trajectory planning problem; (b)generating a first message for each of the cost functions for each corresponding variablebased upon the respective predetermined relationship, the first message indicating a first belief that the corresponding variable has a first value when the optimization problem is solved, the first message having a respective first weight indicating a certainty of thefirst message; (c) generating a second message for each of the variables for each corresponding cost function based upon the respective predetermined relationship, the second message indicating a second belief that the corresponding variable has a second value when theoptimization problem is solved, the second message having a respective second weight indicating a certainty of the second message, wherein each of the first and second weights is one of a zero weight, an infinite weight, and a standard weight; (d) generating a disagreement variable for each corresponding pair of variables and cost functions measuring a disagreement value between the first and second beliefs; (e) repeating steps (b), (c), and (d)until a consensus is formed between the first and second messages based on the cost functions and the variables, a subsequent first message being modified based upon the second message, its corresponding second weight, and the corresponding disagreement variable, and asubsequent second message being modified based upon the first message, its corresponding first weight, and the corresponding disagreement variable; and (f) determining an optimization solution based upon the consensus, wherein the optimization solution comprises a solution to the trajectory planning problem. 10. A device, comprising: a processor coupled to a memory, wherein the processor is programmed to determine an optimization solution to an optimization problem by: (a) receiving the optimization problem, the optimization problem includes a plurality of cost functions and a plurality of variables, the cost functions representing possible costs for values of the variables in the optimization problem, each ofthe cost functions having a predetermined relationship with select ones of the variables,wherein the optimization problem comprises a trajectory planning problem; (b) generating a first message for each of the cost functions for each corresponding variable based upon the respective predetermined relationship, the first message indicating a first belief that the corresponding variable has a first value when the optimization problem is solved, the first message having a respective first weight indicating a certainty of the first message; (c) generating a second message for each of the variables for each corresponding cost function based upon the respective predetermined relationship, the second message indicating a second belief that the corresponding variable has a second value when the optimization problem is solved, the second message having a respective second weight indicating a certainty of the second message, wherein each of the first and second weights is one of a zero weight, an infinite weight, and a standard weight; (d) generating a disagreement variable for each corresponding pair of variables and cost functions measuring a disagreement value between the first and second beliefs; (e) repeating steps (b), (c), and (d) until a consensus is formed between the first and second messages based on the cost functions and thevariables, a subsequent first message being modified based upon the second message, its corresponding second weight, and the corresponding disagreement variable, and a subsequent second message being modified based upon the first message, its corresponding first weight, and the corresponding disagreement variable; and (f) determining the optimization solution based upon the consensus, wherein the optimization solution comprises a solution to thetrajectory planning problem. 18. A non-transitory computer readable storage medium with an executable program stored thereon, wherein the program instructs a microprocessor to perform operations comprising: (a) receiving an optimization problem, the optimization problem includes a plurality of cost functions and a plurality of variables, the cost functions representing possible costs for values of the variables in the optimization problem, each of the cost functions having a predetermined relationship with select ones of the variables, wherein the optimization problem comprises a trajectory planning problem; (b) generating a first message for each of the cost functions for each corresponding variable based upon the respective predetermined relationship, the first message indicating a first belief that the corresponding variables has a first value when the optimization problem is solved,the first message having a respective first weight indicating a certainty of the first message; (c) generating a second message for each of the variables for each corresponding cost function based upon the respective predetermined relationship, the second message indicating a second belief that the corresponding variable has a second value when the optimization is solved, the second message having a respective second weight indicating a certainty of the second message, wherein each of the first and second weights is one of a zero weight, an infinite weight, and a standard weight; (d) generating a disagreement variable foreach corresponding pair of variables and cost functions measuring a disagreement value between the first and second beliefs; (e) repeating steps (b), (c), and (d) until a consensus is formed between the first and second messages based on the cost functions and the variables, a subsequent first message being modified based upon the second message, its corresponding second weight, and the corresponding disagreement variable, and a subsequent second message being modified based upon the first message, its corresponding first weight, andthe corresponding disagreement variable; and (f) determining an optimization solution based upon the consensus, wherein the optimization solution comprises a solution to the trajectory planning problem.